text:
/*
 * Calibrator.cpp
 *
 *  Created on: Dec 13, 2016
 *      Author: nima
 */

#include "RobotToCameraAruco.hpp"
#include "utils/Colors.hpp"
#include "utils/Conversions.hpp"
#include <Eigen/Dense>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/flann.hpp>

#include <pwd.h>

RobotToCameraAruco::RobotToCameraAruco(std::string node_name)
    : calib_finished(true), n(node_name) {

    //    message = "Point at point 1 and press 'space' to start calibration";
    it = new image_transport::ImageTransport(n);

    SetupROS();

    // make 3 points at the corner for calibration
    calib_points_in_board_frame.push_back(cv::Point3f(0.0, 0.0, 0.0));
    calib_points_in_board_frame.push_back(cv::Point3f(0.07, 0.0, 0.0));
    calib_points_in_board_frame.push_back(cv::Point3f(0.0, 0.04, 0.0));

    // declaration of target markers for calib2
    for (uint i=0; i<num_calib_points; i++){
        if(i<num_calib_points/3)
            target.push_back(cv::Point3d((1+i)*(aruco_marker_length_in_meters), aruco_marker_length_in_meters, 0.0));
        if(i>num_calib_points/3-1 && i<2*num_calib_points/3)
            target.push_back(cv::Point3d((i+1-num_calib_points/3)*(aruco_marker_length_in_meters), 2*aruco_marker_length_in_meters, 0.0));
        if(i>-1+2*num_calib_points/3)
            target.push_back(cv::Point3d((i+1-2*num_calib_points/3)*(aruco_marker_length_in_meters), 3*aruco_marker_length_in_meters, 0.0));
    }
};

//-----------------------------------------------------------------------------------
// SetupROS
//-----------------------------------------------------------------------------------

void RobotToCameraAruco::SetupROS() {
    bool all_required_params_found = true;

    n.param<double>("frequency", ros_freq, 25);

    n.param<int>("cam_id", cam_id, 0);
    if(cam_id==1)
        ROS_INFO("Using right camera.");
    else if (cam_id == 0)
        ROS_INFO("Using Left camera.");
    else
        ROS_ERROR("cam_id param can be set as either 0 (for left) or 1 (for "
                          "right)");

    if (n.getParam("visual_axis_length", visual_axis_length))
        ROS_INFO_STREAM(std::string("Using parameter ")
                            << n.resolveName("visual_axis_length").c_str()
                            << " with value " << visual_axis_length);

    if (!n.getParam("number_of_calibration_points", num_calib_points))
        ROS_INFO_STREAM("Parameter"
                            << n.resolveName("number_of_calibration_points").c_str()
                            << ". Default value is used: "
                            << num_calib_points);
    else
        ROS_INFO_STREAM("Using parameter "
                            << n.resolveName("number_of_calibration_points").c_str()
                            << "with value " << num_calib_points
                            << " points per axis. ");

    // if the task_frame_to_robot_frame parameter is present from a previous calibration we can
    // show it
    std::string arm_name;
    (n.getParam("robot_arm_name", arm_name));
    std::stringstream param_name;
    param_name << std::string("/calibrations/task_frame_to_") << arm_name << "_frame";
    std::vector<double> task_frame_to_robot_frame_vec7(7, 0.0);
    if(n.getParam(param_name.str(), task_frame_to_robot_frame_vec7)){

        task_frame_to_robot_frame_param_present = true;
        //Convert and save
        conversions::VectorToKDLFrame(task_frame_to_robot_frame_vec7, task_frame_to_robot_frame);
        std::cout << param_name.str() << " parameter was received as: "
                  << task_frame_to_robot_frame_vec7 << std::endl;
    }

    // ------- load the intrinsic calibration files
    // get home directory
    struct passwd *pw = getpwuid(getuid());
    const char *home_dir = pw->pw_dir;
    std::string cam_name;
    if (n.getParam("cam_name", cam_name)) {

        std::stringstream path;
        path << std::string(home_dir) << "/.ros/camera_info/" << cam_name << "_intrinsics.yaml";
        ReadCameraParameters(path.str(), camera_intrinsics);
    } else
        ROS_ERROR("%s Parameter '%s' is required. Place the intrinsic calibration "
                      "file of each camera in ~/.ros/camera_info/ named as <cam_name>_intrinsics.xml",
                  ros::this_node::getName().c_str(), n.resolveName("cam_name").c_str());

    // a topic name is required for the images
    std::stringstream image_topic_name;
    std::string cam_name_space;
    n.param<std::string>("cam_name_space", cam_name_space, "camera");

    if(cam_id)
        image_topic_name << std::string("/") << cam_name_space <<"/right/image_raw";
    else
        image_topic_name << std::string("/") << cam_name_space <<"/left/image_raw";

    ROS_INFO("Camera images will be read from topic '%s'", image_topic_name.str()
            .c_str());
    camera_image_subscriber = it->subscribe(
            image_topic_name.str(), 1, &RobotToCameraAruco::CameraImageCallback,
            this);

    // a topic name is required for the camera pose
    std::string cam_pose_topic_name;
    if (n.getParam("left_cam_pose_topic_name", cam_pose_topic_name)) {

        // if the topic name is found, check if something is being published
        // on it
        if (!ros::topic::waitForMessage<geometry_msgs::PoseStamped>(
            cam_pose_topic_name, ros::Duration(1))) {
            ROS_WARN("Topic '%s' is not publishing.",
                     cam_pose_topic_name.c_str());
        } else
            ROS_INFO("Reading left camera pose from topic '%s'",
                     cam_pose_topic_name.c_str());
    } else {
        ROS_ERROR("Parameter '%s' is required.", n.resolveName("left_cam_pose_topic_name").c_str());
        all_required_params_found = false;
    }

    // register camera pose subscriber
    left_camera_pose_subscriber =
        n.subscribe(cam_pose_topic_name, 10,
                    &RobotToCameraAruco::LeftCameraPoseCallback, this);

    // a topic name is required for the camera pose
    if (n.getParam("right_cam_pose_topic_name", cam_pose_topic_name)) {

        // if the topic name is found, check if something is being published
        // on it
        if (!ros::topic::waitForMessage<geometry_msgs::PoseStamped>(
            cam_pose_topic_name, ros::Duration(1))) {
            ROS_WARN("Topic '%s' is not publishing.",
                     cam_pose_topic_name.c_str());
        } else
            ROS_INFO("Reading right camera pose from topic '%s'",
                     cam_pose_topic_name.c_str());
    } else {
        ROS_ERROR("Parameter '%s' is required.", n.resolveName("right_cam_pose_topic_name").c_str());
        all_required_params_found = false;
    }

    // register camera pose subscriber
    right_camera_pose_subscriber =
        n.subscribe(cam_pose_topic_name, 10,
                    &RobotToCameraAruco::RightCameraPoseCallback, this);

    // a topic name is required for the robot pose
    std::stringstream robot_pose_topic_name;
    robot_pose_topic_name << std::string("/dvrk/")
            << arm_name << "/position_cartesian_current";
        // if the topic name is found, check if something is being published
        // on it
        if (!ros::topic::waitForMessage<geometry_msgs::PoseStamped>(
            robot_pose_topic_name.str().c_str(), ros::Duration(1))) {
            ROS_ERROR("Topic '%s' is not publishing.",
                      robot_pose_topic_name.str().c_str());
                          all_required_params_found = false;
        } else{
            ROS_INFO("Reading robot pose from topic '%s'",
                     robot_pose_topic_name.str().c_str());
    }
    // register camera pose subscriber
    robot_pose_subscriber =
        n.subscribe(robot_pose_topic_name.str(), 10,
                    &RobotToCameraAruco::RobotPoseCallback, this);

    if (!all_required_params_found)
        throw std::runtime_error("ERROR: some required topics are not set");
}

void RobotToCameraAruco::Calib1DrawCalibrationAxis(cv::String &instructions,
                                                   cv::Mat img) {

    std::ostringstream oss;
    auto n_points = measured_points.size();

    // project the points for both axes
    std::vector<cv::Point3f> axisPoints{
        cv::Point3f(0, 0, 0), cv::Point3f(1, 0, 0) * visual_axis_length,
        cv::Point3f(0, 1, 0) * visual_axis_length,
    };

    std::vector<cv::Point2f> imagePoints;
    cv::projectPoints(axisPoints, task_frame_to_cam_rvec[cam_id],
    task_frame_to_cam_tvec[cam_id],
                      camera_intrinsics.camMatrix,
                      camera_intrinsics.distCoeffs, imagePoints);

    if (n_points < num_calib_points/2) {
        // we are still doing the first axis (x)
        oss << "Take " << (num_calib_points/2 - n_points)
            << " points from the x axis axis.";
        // draw x axis
        cv::line(img, imagePoints[0], imagePoints[1], Colors::Blue, 2, CV_AA);

    } else if (n_points < num_calib_points) {
        // we are at the second axis (y)
        oss << "Take " << (num_calib_points - n_points)
            << " points from the y axis axis.";

        // draw y axis
        cv::line(img, imagePoints[0], imagePoints[2], Colors::Green, 2,
                 CV_AA);
    } else {
        // we are done
        oss << "Received all the points.";
    }

    instructions = oss.str();
}

void RobotToCameraAruco::Calib1SaveCalibrationPoint() {

//    /////// FOR DEBUG
//    measured_points.push_back(Eigen::Vector3d(0.10784, -0.0464285, -0.0861506));
//    measured_points.push_back(Eigen::Vector3d(0.137249, -0.0465126, -0.0856462));
//    measured_points.push_back(Eigen::Vector3d(0.155377, -0.0470039, -0.0853909));
//    measured_points.push_back(Eigen::Vector3d(0.0987962, -0.0195716, -0.0857134));
//    measured_points.push_back(Eigen::Vector3d(0.0984003, -0.00750025, -0.0864288));
//    measured_points.push_back(Eigen::Vector3d(0.098532, -0.00016544, -0.0860863));
//    //

    size_t n_points = measured_points.size();

    if (n_points < num_calib_points) {
        measured_points.push_back(
            Eigen::Vector3d(tool_pose_in_robot_frame.p[0], tool_pose_in_robot_frame.p[1]
                , tool_pose_in_robot_frame.p[2]));
        std::cout << "Added point <" << n_points + 1 << "> : " << tool_pose_in_robot_frame.p[0]
                  << " " << tool_pose_in_robot_frame.p[1] << " " << tool_pose_in_robot_frame.p[2] << std::endl;
    }

    // if we got all the points find the transformation
    if (measured_points.size() == num_calib_points) {
        ROS_INFO("Finding the transformation...");
        // got all the points, find the transformation
        Calib1CalculateTransformation(measured_points, task_frame_to_robot_frame);
        // set the status as done
        calib_finished = true;
    }
}

std::pair<Eigen::Vector3d, Eigen::Vector3d>
RobotToCameraAruco::FindAxis(Eigen::MatrixXd axis_points) {

    auto n_points = axis_points.rows();

    // convert to std vector
    std::vector<Eigen::Vector3d> points;
    for (size_t i = 0; i < n_points; ++i)
        points.push_back(axis_points.row(i));

    // fit the points and normalize
    auto axis_pair = FitLineToPoints(points);

    Eigen::Vector3d axis = axis_pair.second;
    axis.normalize();

    // finding the correct direction of the axis
    if (axis.dot(axis_points.row(1) - axis_points.row(0)) < 0) {
        axis = -axis;
        std::cout << "flipped axis" << std::endl;
    }
    return std::make_pair(axis_pair.first, axis);
}

void RobotToCameraAruco::Calib1CalculateTransformation(
    const std::vector<Eigen::Vector3d> axis_points, KDL::Frame & transformation) {

    auto n_points = axis_points.size();
    auto n_points_per_axis = (n_points) / 2;

    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> points(n_points, 3);
    for (uint i = 0; i < n_points; i++)
        points.row(i) = axis_points[i];

    // ---------------  fit plane to the points
    auto origin_axis_pair = FitPlaneToPoints(axis_points);
    Eigen::Vector3d plane_origin = origin_axis_pair.first;
    Eigen::Vector3d plane_normal = origin_axis_pair.second;

    // --------------- project the points on the plane
    Eigen::MatrixXd centered = points.rowwise() - plane_origin.transpose();

    // the dot product of the vector from each point to center and the plane
    // normal
    auto dist = centered * plane_normal;

    Eigen::MatrixXd projection_vectors(n_points, 3);

    for (uint iter = 0; iter < n_points; iter++)
        projection_vectors.row(iter) = dist(iter) * plane_normal;

    Eigen::MatrixXd points_projected = points - projection_vectors;

    // ---------------  fit the x axis
    auto x_axis_pair = FindAxis(points_projected.topRows(n_points_per_axis));
    Eigen::Vector3d x_axis_point = x_axis_pair.first;
    Eigen::Vector3d x_axis = x_axis_pair.second;

    //  --------------- fit the y axis
    auto y_axis_pair =
        FindAxis(points_projected.bottomRows(n_points_per_axis));
    Eigen::Vector3d y_axis_point = y_axis_pair.first;
    Eigen::Vector3d y_axis = y_axis_pair.second;

    //  --------------- find the z axis
    Eigen::Vector3d z_axis = x_axis.cross(y_axis);
    z_axis.normalize();

    // make sure y is normal to xz plane
    y_axis = z_axis.cross(x_axis);

    // Find the intersection of x and y
    // solving as a least-square problem Rp=q
    // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(3, 3);
    Eigen::MatrixXd R =
        (I - x_axis * x_axis.transpose()) + (I - y_axis * y_axis.transpose());

    Eigen::VectorXd q = (I - x_axis * x_axis.transpose()) * x_axis_point +
                        (I - y_axis * y_axis.transpose()) * y_axis_point;

    Eigen::VectorXd origin =
        R.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(q);

    // convert to opencv matrix and vector
    // Todo use frameM directly
    cv::Matx33d rot_mat;
    for (uint i = 0; i < 3; i++) {
        rot_mat(i, 0) = x_axis(i);
        rot_mat(i, 1) = y_axis(i);
        rot_mat(i, 2) = z_axis(i);
        transformation.p[i] = origin(i);
    }
    conversions::Matx33dToKdlRot(rot_mat, transformation.M);

}

void RobotToCameraAruco::Calib2DrawTarget(cv::String &instructions, cv::Mat img) {

    cv::projectPoints(target, task_frame_to_cam_rvec[cam_id],
                      task_frame_to_cam_tvec[cam_id], camera_intrinsics.camMatrix,
                      camera_intrinsics.distCoeffs, calib_points_screen);

    std::ostringstream oss;
    oss << "Take the tool tip to the target point then press s";
    for (uint i=0; i<num_calib_points; i++){
        cv::circle(img, calib_points_screen[i], 6, cv::Scalar(255, 255, 0),
                   1, CV_AA);
    }
    // draw actual target with a different colour
    cv::circle(img, calib_points_screen[meas_points.size()], 6, cv::Scalar(0, 0, 255),
               1, CV_AA);
    instructions = oss.str();
}

void RobotToCameraAruco::Calib2SaveCalibrationPoint() {

    size_t num_p = meas_points.size();
    if (num_p < num_calib_points) {
        // save the position of the end effector
        meas_points.push_back(
            Eigen::Vector3d(
                tool_pose_in_robot_frame.p[0], tool_pose_in_robot_frame.p[1],
                tool_pose_in_robot_frame.p[2]));
    }
    if (num_p == num_calib_points) {
        Calib2CalculateTransformation();
    }
}

void RobotToCameraAruco::Calib2CalculateTransformation() {

    size_t num_p = target.size();

    for (uint i = 0; i < num_p; i++) {
        meas_points_mat.col(i) = meas_points[i];
        target_mat(0,i) = target[i].x;
        target_mat(1,i) = target[i].y;
        target_mat(2,i) = target[i].z;
    }

    auto temp = Eigen::umeyama(target_mat, meas_points_mat, 0);

    // converting Eigen::Matrix into KDL::Frame
    cv::Matx33d rot_mat;
    for (uint i = 0; i < 3; i++) {
        rot_mat(i, 0) = temp(i, 0);
        rot_mat(i, 1) = temp(i, 1);
        rot_mat(i, 2) = temp(i, 2);
        task_frame_to_robot_frame.p[i] = temp(i, 3);
    }
    conversions::Matx33dToKdlRot(rot_mat, task_frame_to_robot_frame.M);
    ROS_INFO_STREAM("  -> Board To PSM Transformation: \n" << temp << std::endl);
    calib_finished = true;
}

void RobotToCameraAruco::SetROSParameters() {

    std::string arm_name;
    (n.getParam("robot_arm_name", arm_name));

    std::stringstream param_name;
    param_name << std::string("/calibrations/task_frame_to_") << arm_name << "_frame";

    std::vector<double> vec7(7, 0.0);
    conversions::KDLFrameToVector(task_frame_to_robot_frame, vec7);
    n.setParam(param_name.str(), vec7);
    std::cout<< param_name.str()<< " parameter was set to: "<< vec7 << std::endl;

    param_name.str("");
    param_name << std::string("/calibrations/task_frame_to_left_cam_frame");
    conversions::KDLFrameToVector(task_frame_to_cam_frame[0], vec7);
    n.setParam(param_name.str(), vec7);
    std::cout<< param_name.str()<< " parameter was set to: "<< vec7 << std::endl;

    param_name.str("");
    param_name << std::string("/calibrations/task_frame_to_right_cam_frame");
    conversions::KDLFrameToVector(task_frame_to_cam_frame[1], vec7);
    n.setParam(param_name.str(), vec7);
    std::cout<< param_name.str()<< " parameter was set to: "<< vec7 << std::endl;

    // print the pose of board_to_cam
    auto camera_frame_to_robot_frame = task_frame_to_robot_frame *
    task_frame_to_cam_frame[cam_id].Inverse();
    conversions::KDLFrameToVector(camera_frame_to_robot_frame, vec7);
    std::cout<< " camera_frame_to_robot_frame is: "<< vec7 << std::endl;

}

void RobotToCameraAruco::Reset() {
    calib_finished = false;
}

void RobotToCameraAruco::ReadCameraParameters(const std::string file_path,
                                              CameraIntrinsics & camera) {
    cv::FileStorage fs(file_path, cv::FileStorage::READ);
    ROS_INFO("Reading camera intrinsic data from: '%s'",file_path.c_str());

    if (!fs.isOpened())
        throw std::runtime_error("Unable to read the camera parameters file.");

    fs["camera_matrix"] >> camera.camMatrix;
    fs["distortion_coefficients"] >> camera.distCoeffs;

    // check if we got osomething
    if(camera.distCoeffs.empty()){
        ROS_ERROR("distortion_coefficients was not found in '%s' ", file_path.c_str());
        throw std::runtime_error("ERROR: Intrinsic camera parameters not found.");
    }
    if(camera.camMatrix.empty()){
        ROS_ERROR("camera_matrix was not found in '%s' ", file_path.c_str());
        throw std::runtime_error("ERROR: Intrinsic camera parameters not found.");
    }

}

void RobotToCameraAruco::CameraImageCallback(
    const sensor_msgs::ImageConstPtr &msg) {
    try {
        image_msg = cv_bridge::toCvCopy(msg, "bgr8")->image;
    } catch (cv_bridge::Exception &e) {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.",
                  msg->encoding.c_str());
    }
}

void RobotToCameraAruco::LeftCameraPoseCallback(
        const geometry_msgs::PoseStamped::ConstPtr &msg) {

    // converting to frame and rvec/tvec
    tf::poseMsgToKDL(msg->pose, task_frame_to_cam_frame[0]);
    conversions::KDLFrameToRvectvec(task_frame_to_cam_frame[0],
                                    task_frame_to_cam_rvec[0],
                                    task_frame_to_cam_tvec[0]);
}

void RobotToCameraAruco::RightCameraPoseCallback(
        const geometry_msgs::PoseStamped::ConstPtr &msg) {

    // converting to frame and rvec/tvec
    tf::poseMsgToKDL(msg->pose, task_frame_to_cam_frame[1]);
    conversions::KDLFrameToRvectvec(task_frame_to_cam_frame[1],
                                    task_frame_to_cam_rvec[1],
                                    task_frame_to_cam_tvec[1]);
}

cv::Mat &RobotToCameraAruco::Image(ros::Duration timeout) {
    ros::Rate loop_rate(1);
    ros::Time timeout_time = ros::Time::now() + timeout;

    while (image_msg.empty()) {
        ros::spinOnce();
        loop_rate.sleep();

        if (ros::Time::now() > timeout_time) {
            ROS_WARN("Timeout: No new Image received.");
        }
    }

    return image_msg;
}

void RobotToCameraAruco::RobotPoseCallback(
    const geometry_msgs::PoseStamped::ConstPtr &msg) {
    // save the pose in a kdl frame
    tf::poseMsgToKDL(msg->pose, tool_pose_in_robot_frame);
}

std::ostream &operator<<(std::ostream &out, const std::vector<double> &vect) {
    for (unsigned int iter = 0; iter < vect.size(); ++iter) {
        out << "[" << iter << "]: " << vect.at(iter) << "\t";
    }

    return out;
}

ext: cpp

max_stars_repo_name: neemoh/ART

max_issues_repo_name: liuxia-zju/ATAR

max_forks_repo_name: liuxia-zju/ATAR


text:
/**
 * Copyright (c) 2018, University Osnabrück
 * 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 University Osnabrück 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 University Osnabrück 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.
 */

/*
 * OptionsDMC.hpp
 *
 *  Created on: May 13, 2020
 *      Author: Benedikt SChumacher
 */
#ifndef OPTIONSDMC_H_
#define OPTIONSDMC_H_

#include "lvr2/config/BaseOption.hpp"

#include <boost/program_options.hpp>
#include <float.h>
#include <iostream>
#include <string>
#include <vector>

using std::cout;
using std::endl;
using std::ostream;
using std::string;
using std::vector;

using namespace lvr2;

namespace dmc_reconstruction
{

class Options : public BaseOption
{
  public:
    Options(int argc, char** argv);
    virtual ~Options();

    string getInputFileName() const;

    int getMaxLevel() const;

    float getMaxError() const;

    /*
     * prints information about needed command-line-inputs e.g: input-file (ply)
     */
    bool printUsage() const;

    int getKd() const;

    int getKn() const;

    int getKi() const;

    int getNumThreads() const;

    string getPCM() const;

    bool useRansac() const;

    string getScanPoseFile() const;

  private:
    /// The maximum allows octree level
    int m_maxLevel;

    /// The max allowed error between points and surfaces in an octree cell
    float m_maxError;

    /// The number of neighbors for distance function evaluation
    int m_kd;

    /// The number of neighbors for normal estimation
    int m_kn;

    /// The number of neighbors for normal interpolation
    int m_ki;

    /// The number of threads to use
    int m_numThreads;

    /// The used point cloud manager
    string m_pcm;
};

/// Output the Options - overloaded output Operator
inline ostream& operator<<(ostream& os, const Options& o)
{
    // o.printTransformation(os);

    cout << "##### InputFile-Name: " << o.getInputFileName() << endl;
    cout << "##### Max Level: " << o.getMaxLevel() << endl;
    cout << "##### Max Error: " << o.getMaxError() << endl;

    cout << "##### PCM: " << o.getPCM() << endl;
    cout << "##### KD: " << o.getKd() << endl;
    cout << "##### KI: " << o.getKi() << endl;
    cout << "##### KN: " << o.getKn() << endl;

    return os;
}
} // namespace dmc_reconstruction

#endif // OPTIONSDMC_H_

ext: hpp

max_stars_repo_name: uos/lvr

max_issues_repo_name: uos/lvr

max_forks_repo_name: uos/lvr


text:
// Copyright (c) 2019 Christopher A. Taylor.  All rights reserved.

/*
    Depth mesh generator

    Takes 16-bit depth image and calibration as input.
    Produces OpenGL-compatible mesh x,y,z,u,v float vertices and a triangle index buffer.
    Zeroes out depth image values that cannot be used for the mesh for better compression.
*/

#pragma once

#include "DepthCalibration.hpp"

#include <Eigen/Eigen>

#include <vector>

namespace core {

//------------------------------------------------------------------------------
// Datatypes

// Cull parts of the mesh outside of a clipped region of interest
struct ClipRegion
{
    Eigen::Matrix4f Extrinsics;

    // Clip limits
    float Radius = 0.f;
    float Floor = 0.f;
    float Ceiling = 0.f;
};

struct ImageCropRegion
{
    unsigned CropX = 0, CropY = 0;
    unsigned CropW = 0, CropH = 0;

    // Grow to include the provided region
    // Returns true if it had to grow
    bool Grow(const ImageCropRegion& other);
};

//------------------------------------------------------------------------------
// DepthMesher

// After initialization this is safe to use from multiple threads in parallel.
// The idea would be to have one of these for each capture device.
class DepthMesher
{
public:
    // Must be called before other functions
    void Initialize(const CameraCalibration& calibration);

    // OpenGL-compatible x, y, z, u, v coordinates without padding.
    // depth: Must match the dimensions from Initialize().
    // Zeroes out depth entries that are not useful.
    // Optional: Zeroes out depth entries that are outside the clip region.
    // Optional: Provides image crop region.
    // face_painting_fix: Remove nearfield objects from the backdrop.
    //      Recommended for close-ups, should disable for 2 meter+ stand-off.
    void GenerateCoordinates(
        uint16_t* depth,
        const ClipRegion* clip,
        //ImageCropRegion* crop,
        std::vector<float>& coordinates,
        bool face_painting_fix = true,
        bool cull_depth = true);

    // OpenGL-compatible 3 indices for each triangle.
    // Call after GenerateCoordinates().
    // Triangle vertices are wound such that the right-hand rule yields
    // normals pointing towards the camera.
    void GenerateTriangleIndices(
        const uint16_t* depth,
        std::vector<uint32_t>& indices);

    // Get image crop from mesh clip region
    void CalculateCrop(
        const ClipRegion& clip,
        ImageCropRegion& crop);

protected:
    CameraCalibration Calibration;

    std::vector<float> DepthLookup;
};

//------------------------------------------------------------------------------
// TemporalDepthFilter

/*
    Applies a One Euro filter to depth video.

    Keeps a recent history for each depth pixel, and for pixels that do not
    change drastically, it applies median filter smoothing to restore missing
    data and to improve depth accuracy.

    Applications:
        Improving accuracy of depth meshes for extrinsics calibration of cameras
        by using the Iterative Closest Points (ICP) method to align the meshes.
        Improving accuracy of depth meshes for static background scene objects.
        Expected to be applied on the capture server.
*/
class TemporalDepthFilter
{
public:
    void Filter(uint16_t* depth, int w, int h);

protected:
    static const int kStride = 8;

    int Width = 0, Height = 0, Count = 0, Index = 0;
    std::vector<uint16_t> History;
};

//------------------------------------------------------------------------------
// DepthEdgeFilter

/*
    This filter cuts away edges of a mesh where there is the most uncertainty.

    The first pass through the image will filter large changes in depth as edges
    by setting the closer depth pixel to zero.  In this pass we also construct
    an integral image (see below).

    The second pass through the image we query the integral image to determine
    the number of neighbors for each depth image pixel.  We cull any pixel with
    fewer than 6 neighbors, which is considered an edge.
*/

class DepthEdgeFilter
{
public:
    void Filter(uint16_t* depth, int w, int h);

protected:
    /*
        Integral image counting the number of non-zero neighbors for each depth
        image pixel.  It will overflow and wrap around but since we are only
        querying the I.I. for small regions it is still unambiguous.

        Good reference for this data structure:
        https://www.mathworks.com/help/images/ref/integralimage.html
    */
    std::vector<uint16_t> IntegralImage;
};

} // namespace core

max_stars_repo_name: catid/xrcap

max_issues_repo_name: catid/xrcap

max_forks_repo_name: catid/xrcap


text:
#include <chrono>
#include <cmath>
#include <exception>
#include <thread>
#include <vector>

#include <boost/log/trivial.hpp>

#include "mimir/algorithm/FishSchool.hpp"
#include "mimir/StateMachineFwd.hpp"
#include "mimir/FkinDds.hpp"

#include <casadi/casadi.hpp>

namespace mimir
{
  namespace algorithm
  {
    class FishSchool::Impl
    {
    public:
      Impl(
          const YAML::Node& config,
          dds::pub::Publisher publisher,
          dds::sub::Subscriber subscriber) :
        inputReader(dds::core::null),
        outputWriter(dds::core::null),
        initialPosition(dds::core::null),
        initialEuler(dds::core::null)
      {

        // Setup input reader
        const auto input_config = config["inputs"]["fish_ctrl"];
        const auto input_age = input_config["max_age_ms"].as<std::int64_t>();

        auto readerQos = subscriber.default_datareader_qos();
        //readerQos << dds::core::policy::Durability::TransientLocal()
        //          << dds::core::policy::Reliability::Reliable();

        if(input_age > 0)
          readerQos << org::opensplice::core::policy::ReaderLifespan(
              true,
              dds::core::Duration::from_millisecs(input_age));

        const auto topicName = input_config["topic"].as<std::string>();
        const auto id = input_config["id"].as<std::string>();
        auto topic = dds::topic::Topic<fkin::IdVec3d>(
            subscriber.participant(),
            topicName);
        auto filter = dds::topic::Filter("id = %0", {id});

        inputReader = dds::sub::DataReader<fkin::IdVec3d>(
            subscriber,
            dds::topic::ContentFilteredTopic<fkin::IdVec3d>(
                topic,
                topicName + id,
                filter),
            readerQos);

        // Setup output writer
        auto writerQos = publisher.default_datawriter_qos();
        //writerQos << dds::core::policy::Durability::TransientLocal();

        outputWriter = dds::pub::DataWriter<fkin::Kinematics6D>(
            publisher,
            dds::topic::Topic<fkin::Kinematics6D>(
                publisher.participant(),
                config["outputs"]["kinematics"]["topic"].as<std::string>()),
            writerQos);

        outputId = config["outputs"]["kinematics"]["id"].as<std::string>();

        // Setup initial condition reader
        readerQos << org::opensplice::core::policy::ReaderLifespan(
            false,
            dds::core::Duration::infinite());

        const auto initCondConfig = config["initial_conditions"]["position"];
        const auto initTopicName = initCondConfig["topic"].as<std::string>();
        const auto idInit = initCondConfig["id"].as<std::string>();
        waitPosMs = initCondConfig["max_wait_ms"].as<std::int32_t>();
        topic = dds::topic::Topic<fkin::IdVec3d>(
            subscriber.participant(),
            initTopicName);
        filter = dds::topic::Filter("id = %0", {idInit});

        initialPosition = dds::sub::DataReader<fkin::IdVec3d>(
            subscriber,
            dds::topic::ContentFilteredTopic<fkin::IdVec3d>(
                topic,
                initTopicName + idInit,
                filter),
            readerQos);

        fallbackInitPosition =
             config["initial_conditions"]["position"]["fallback"]
             .as<std::vector<double>>();

        const auto initCondConfig2 = config["initial_conditions"]["euler"];
        const auto initTopicName2 = initCondConfig2["topic"].as<std::string>();
        const auto idInit2 = initCondConfig2["id"].as<std::string>();
        waitEulerMs = initCondConfig["max_wait_ms"].as<std::int32_t>();
        topic = dds::topic::Topic<fkin::IdVec3d>(
            subscriber.participant(),
            initTopicName2);
        filter = dds::topic::Filter("id = %0", {idInit2});

        initialEuler = dds::sub::DataReader<fkin::IdVec3d>(
            subscriber,
            dds::topic::ContentFilteredTopic<fkin::IdVec3d>(
                topic,
                initTopicName2 + idInit2,
                filter),
            readerQos);

        fallbackInitEuler =
         config["initial_conditions"]["euler"]["fallback"]
         .as<std::vector<double>>();

        using namespace casadi;

        auto p = SX::sym("p", 3); // N E D
        auto eu = SX::sym("Euler", 2); // pitch and yaw
        auto x_sym = SX::vertcat({p,eu});
        auto RT = SX::zeros(5,5);
        auto vb = SX::sym("vb", 3); // Body linear velocity
        auto wb = SX::sym("wb", 2); // Body angular velocity (pitch yaw)
        auto zd = SX::sym("z_d", 1); // Desired depth

        auto rotYTheta = SX::vertcat(
            {
             SX::horzcat({cos(eu(0)),0,sin(eu(0))}),
             SX::horzcat({0,1,0}),
             SX::horzcat({-sin(eu(0)),0,cos(eu(0))})
            });

        auto rotZPsi = SX::vertcat(
            {
             SX::horzcat({cos(eu(1)),-sin(eu(1)), 0}),
             SX::horzcat({sin(eu(1)),cos(eu(1)), 0}),
             SX::horzcat({0,0,1})
            });
        auto rot = mtimes(rotZPsi, rotYTheta);

        RT(Slice(0,3),Slice(0,3)) = rot;
        RT(Slice(3,5),Slice(3,5)) =
         SX::vertcat({
                      SX::horzcat({1,0}),
                      SX::horzcat({0,1/cos(eu(0))})});

        auto pitch_rate_ctrl = 0.1*(p(2) - zd)*cos(2*eu(0)) - eu(0);
        auto u_sym = SX::vertcat({vb, wb(1), zd}); // 3d velocity, rate of turn, desired depth
        auto u_vec = SX::vertcat({vb,pitch_rate_ctrl, wb(1)});
        auto rhs = mtimes(RT,u_vec);

        SXDict ode = {{"x", x_sym}, {"p", u_sym}, {"ode", rhs}};

        model = casadi::integrator(
            "fish_school", "cvodes", ode,
            {{"tf", config["time_step_ms"].as<double>()/1000.}});
        u = casadi::DM({1.,0.,0.,0.,0.});

      }
      ~Impl() {}
      dds::sub::DataReader<fkin::IdVec3d> inputReader;
      dds::pub::DataWriter<fkin::Kinematics6D> outputWriter;
      dds::sub::DataReader<fkin::IdVec3d> initialPosition;
      dds::sub::DataReader<fkin::IdVec3d> initialEuler;
      std::chrono::steady_clock::time_point t0;
      int32_t waitPosMs, waitEulerMs;
      std::vector<double> fallbackInitPosition, fallbackInitEuler;
      std::string outputId;
      casadi::Function model;
      casadi::DM u;
      casadi::DM x_k;
    };

    FishSchool::FishSchool(
        const YAML::Node& config,
        boost::statechart::fifo_scheduler<>& scheduler,
        boost::statechart::fifo_scheduler<>::processor_handle machine,
        dds::pub::Publisher publisher,
        dds::sub::Subscriber subscriber) :
      m_impl( new FishSchool::Impl(config, publisher, subscriber)),
      m_scheduler(scheduler),
      m_stateMachine(machine),
      m_time_step(std::chrono::milliseconds(config["time_step_ms"].as<std::int32_t>())),
      m_next_step(std::chrono::steady_clock::now()),
      m_config(config)
      // Allow for simulation speed factor
    {}

    FishSchool::~FishSchool() = default;
    void FishSchool::solve(const std::atomic<bool>& cancel_token)
    {
      try
      {
        BOOST_LOG_TRIVIAL(trace) << "FishSchool solve..";

        // Publish x_k(t_now) (we do not publish future samples)
        const auto x_k = std::vector<double>(m_impl->x_k);
        const auto u_k = std::vector<double>(m_impl->u);

        m_impl->outputWriter << fkin::Kinematics6D(
            m_impl->outputId,
            fkin::Vector3d(x_k[0], x_k[1], x_k[2]),
            fkin::Vector3d(u_k[0], u_k[1], u_k[2]),
            fkin::Vector3d(0., x_k[3], x_k[4]));

        // Fetch input, sample and hold
        auto samples = m_impl->inputReader.read();

        // Set new desired value
        if(samples.length() > 0)
        {
          auto sample = (--samples.end());
          if(sample->info().valid())
          {
            auto sD = sample->data();
            m_impl->u = casadi::DM({sD.vec().x(), 0., 0., sD.vec().z(), sD.vec().y()});
            BOOST_LOG_TRIVIAL(trace) << "Got a sample: " << m_impl->u;
          }
        }
        else
        {
          BOOST_LOG_TRIVIAL(trace) << "No valid input sample for " << name() << ", using old";
        }

        if(cancel_token)
        {
          BOOST_LOG_TRIVIAL(info) << name() << " interrupted";
          event(new mimir::EvInterrupt());
          return;
        }

        // Integrate
        auto result = m_impl->model(
            casadi::DMDict( {{"x0", m_impl->x_k}, {"p", m_impl->u}} ));

        if(cancel_token)
        {
          BOOST_LOG_TRIVIAL(info) << name() << " interrupted";
          event(new mimir::EvInterrupt());
          return;
        }

        // Update output (not published yet)
        m_impl->x_k = result["xf"];
        step_time();
        event(new mimir::EvReady());

      }
      catch(casadi::CasadiException &e)
      {
        BOOST_LOG_TRIVIAL(fatal) << name() << " Casadi Exception: " << e.what();
        event(new mimir::EvError());
        throw;
      }
      catch (...)
      {
        // Need to post event in case of exception so that the state machine
        // knows that the job has finished.
        BOOST_LOG_TRIVIAL(fatal) << name() << " exception thrown";
        event(new mimir::EvError());
        throw;
      }
    }
    void FishSchool::initialize(const std::atomic<bool>&)
    {
      try
      {
        BOOST_LOG_TRIVIAL(debug) << name() << " is initializing";

        auto waitPos = dds::core::cond::WaitSet();
        auto readCondPos = dds::sub::cond::ReadCondition(
            m_impl->initialPosition,
            dds::sub::status::DataState::new_data());

        waitPos.attach_condition(readCondPos);

        auto waitEuler = dds::core::cond::WaitSet();
        auto readCondEuler = dds::sub::cond::ReadCondition(
            m_impl->initialEuler,
            dds::sub::status::DataState::new_data());

        waitEuler.attach_condition(readCondEuler);

        casadi::DM pos, euler;
        try
        {
          waitPos.wait(dds::core::Duration::from_millisecs(m_impl->waitPosMs));
          auto samples = m_impl->initialPosition.read();

          // Set new desired value
          if(samples.length() > 0)
          {
            auto sample = (--samples.end());
            if(sample->info().valid())
            {
              auto sD = sample->data();
              pos = casadi::DM({sD.vec().x(), sD.vec().y(), sD.vec().z()});
              BOOST_LOG_TRIVIAL(trace) << "Initial position received: " << pos;
            }
          }
        }
        catch (dds::core::TimeoutError &)
        {
          BOOST_LOG_TRIVIAL(trace) << "Falling back to pre-defined initial position "
                                   << m_impl->fallbackInitPosition;
          pos = casadi::DM(std::vector<double>(m_impl->fallbackInitPosition));

        }
        try
        {
          waitEuler.wait(dds::core::Duration::from_millisecs(m_impl->waitEulerMs));
          auto samples = m_impl->initialEuler.read();

          // Set new desired value
          if(samples.length() > 0)
          {
            auto sample = (--samples.end());
            if(sample->info().valid())
            {
              auto sD = sample->data();
              euler = casadi::DM(std::vector<double>{sD.vec().y(), sD.vec().z()});
              BOOST_LOG_TRIVIAL(trace) << "Initial euler received: " << euler;
            }
          }
        }
        catch (dds::core::TimeoutError &)
        {
          BOOST_LOG_TRIVIAL(trace) << "Falling back to pre-defined initial euler";
          euler = casadi::DM(std::vector<double>{m_impl->fallbackInitEuler[1], m_impl->fallbackInitEuler[2]});
        }

        BOOST_LOG_TRIVIAL(trace) << "Pos: " << pos;
        BOOST_LOG_TRIVIAL(trace) << "Euler: " << euler;

        m_impl->x_k = casadi::DM::vertcat({pos, euler});

        m_next_step = std::chrono::steady_clock::now();
        event(new mimir::EvReady());
      }
      catch (...)
      {
        BOOST_LOG_TRIVIAL(fatal) << name() << " initializing threw exception";
        event(new mimir::EvError());
        throw;
      }
    }
    void FishSchool::timer(const std::atomic<bool>& cancel_token)
    {
      // probably want external time point to wait until.
      typedef std::chrono::milliseconds scm;
      auto now = std::chrono::steady_clock::now();
      auto time_p = m_next_step;
      auto timeleft = std::chrono::duration_cast<scm>(time_p - now).count();
      BOOST_LOG_TRIVIAL(trace) << "Time left: " << timeleft;

      auto fraction = (m_time_step/100 > scm(0) ? m_time_step/100 :
       (m_time_step/50 > scm(0) ? m_time_step/50 :
        (m_time_step/25 > scm(0) ? m_time_step/25 :
         (m_time_step/10 > scm(0) ? m_time_step/10 :
          (m_time_step/2 > scm(0) ?  m_time_step/2  : scm(1))))));

      while(now < time_p)
      {
        auto diff = time_p - now;
        std::this_thread::sleep_for(diff < fraction ? diff : fraction);
        if(cancel_token)
          break;
        now = std::chrono::steady_clock::now();
      }

      if(cancel_token)
      {
        BOOST_LOG_TRIVIAL(trace) << "Timer Canceled";
        event(new mimir::EvInterrupt());
      }
      else
      {
        BOOST_LOG_TRIVIAL(trace) << "Timeout";
        event(new mimir::EvTimeout());
      }
    }

    void FishSchool::event(boost::statechart::event_base * const event)
    {
      // who is responsible for the pointer?
      m_scheduler.queue_event(
          m_stateMachine,
          make_intrusive(event));
    }

  }

}

max_stars_repo_name: sintef-ocean/mimir

max_issues_repo_name: sintef-ocean/mimir

max_forks_repo_name: sintef-ocean/mimir


text:
/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include <itkImageFileWriter.h>
#include <itkResampleImageFilter.h>
#include <itkOdfMaximaExtractionFilter.h>

#include <mitkImage.h>
#include <mitkOdfImage.h>
#include <mitkImageCast.h>
#include <mitkImageToItk.h>
#include <mitkTensorImage.h>

#include <mitkCoreObjectFactory.h>
#include "mitkCommandLineParser.h"
#include <itkShCoefficientImageImporter.h>
#include <itkFlipImageFilter.h>
#include <mitkLexicalCast.h>
#include <boost/algorithm/string.hpp>
#include <mitkLocaleSwitch.h>
#include <mitkIOUtil.h>

template<int shOrder>
int StartPeakExtraction(int argc, char* argv[])
{
  mitkCommandLineParser parser;
  parser.setArgumentPrefix("--", "-");
  parser.addArgument("image", "i", mitkCommandLineParser::InputFile, "Input image", "sh coefficient image", us::Any(), false);
  parser.addArgument("outroot", "o", mitkCommandLineParser::OutputDirectory, "Output directory", "output root", us::Any(), false);
  parser.addArgument("mask", "", mitkCommandLineParser::InputFile, "Mask", "mask image");
  parser.addArgument("normalization", "", mitkCommandLineParser::Int, "Normalization", "0=no norm, 1=max norm, 2=single vec norm", 1, true);
  parser.addArgument("numpeaks", "", mitkCommandLineParser::Int, "Max. number of peaks", "maximum number of extracted peaks", 2, true);

  parser.addArgument("rel_peakthr", "", mitkCommandLineParser::Float, "Relative peak threshold", "peak threshold relative to largest peak", 0.4, true);
  parser.addArgument("abs_peakthr", "", mitkCommandLineParser::Float, "Absolute peak threshold", "absolute peak magnitude threshold", 0.03, true);
  parser.addArgument("angular_thr", "", mitkCommandLineParser::Float, "Angular threshold", "in degree", 15);

  parser.addArgument("shConvention", "", mitkCommandLineParser::String, "Use specified SH-basis", "use specified SH-basis (MRtrix, FSL)", std::string("MRtrix"), true);

  parser.addArgument("flipX", "", mitkCommandLineParser::Bool, "Flip X", "Flip peaks in x direction");
  parser.addArgument("flipY", "", mitkCommandLineParser::Bool, "Flip Y", "Flip peaks in y direction");
  parser.addArgument("flipZ", "", mitkCommandLineParser::Bool, "Flip Z", "Flip peaks in z direction");
  parser.addArgument("scale_by_gfa", "", mitkCommandLineParser::Bool, "Scale by GFA", "Scale ODF values and peaks by GFA");

  parser.setCategory("Preprocessing Tools");
  parser.setTitle("Peak Extraction");
  parser.setDescription("");
  parser.setContributor("MIC");

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  // mandatory arguments
  std::string imageName = us::any_cast<std::string>(parsedArgs["image"]);
  std::string outRoot = us::any_cast<std::string>(parsedArgs["outroot"]);

  // optional arguments
  std::string maskImageName("");
  if (parsedArgs.count("mask"))
    maskImageName = us::any_cast<std::string>(parsedArgs["mask"]);

  int normalization = 1;
  if (parsedArgs.count("normalization"))
    normalization = us::any_cast<int>(parsedArgs["normalization"]);

  int numPeaks = 2;
  if (parsedArgs.count("numpeaks"))
    numPeaks = us::any_cast<int>(parsedArgs["numpeaks"]);

  float rel_peakthr = 0.4;
  if (parsedArgs.count("rel_peakthr"))
    rel_peakthr = us::any_cast<float>(parsedArgs["rel_peakthr"]);

  float abs_peakthr = 0.03;
  if (parsedArgs.count("abs_peakthr"))
    abs_peakthr = us::any_cast<float>(parsedArgs["abs_peakthr"]);

  float angular_thr = 15;
  if (parsedArgs.count("angular_thr"))
    angular_thr = us::any_cast<float>(parsedArgs["angular_thr"]);
  angular_thr = cos((float)angular_thr*itk::Math::pi / 180);

  bool scale_by_gfa = false;
  if (parsedArgs.count("scale_by_gfa"))
    scale_by_gfa = us::any_cast<bool>(parsedArgs["scale_by_gfa"]);

  bool flipX = false;
  if (parsedArgs.count("flipX"))
    flipX = us::any_cast<bool>(parsedArgs["flipX"]);

  bool flipY = false;
  if (parsedArgs.count("flipY"))
    flipY = us::any_cast<bool>(parsedArgs["flipY"]);

  bool flipZ = false;
  if (parsedArgs.count("flipZ"))
    flipZ = us::any_cast<bool>(parsedArgs["flipZ"]);

  std::cout << "image: " << imageName;
  std::cout << "outroot: " << outRoot;
  if (!maskImageName.empty())
    std::cout << "mask: " << maskImageName;
  else
    std::cout << "no mask image selected";
  std::cout << "numpeaks: " << numPeaks;
  std::cout << "peakthres: " << rel_peakthr;
  std::cout << "abspeakthres: " << abs_peakthr;
  std::cout << "shOrder: " << shOrder;

  try
  {
    mitk::Image::Pointer image = mitk::IOUtil::Load<mitk::Image>(imageName);
    mitk::Image::Pointer mask = mitk::IOUtil::Load<mitk::Image>(maskImageName);

    typedef itk::Image<unsigned char, 3>  ItkUcharImgType;
    typedef itk::OdfMaximaExtractionFilter< float, shOrder, 20242 > MaximaExtractionFilterType;
    typename MaximaExtractionFilterType::Pointer peak_extraction_filter = MaximaExtractionFilterType::New();

    ItkUcharImgType::Pointer itkMaskImage = nullptr;
    if (mask.IsNotNull())
    {
      try{
        itkMaskImage = ItkUcharImgType::New();
        mitk::CastToItkImage(mask, itkMaskImage);
        peak_extraction_filter->SetMaskImage(itkMaskImage);
      }
      catch(...)
      {

      }
    }

    if (parsedArgs.count("shConvention"))
    {
      std::string convention = us::any_cast<std::string>(parsedArgs["shConvention"]).c_str();
      if ( convention=="FSL" )
        peak_extraction_filter->SetToolkit(MaximaExtractionFilterType::FSL);
      else
        peak_extraction_filter->SetToolkit(MaximaExtractionFilterType::MRTRIX);
    }
    else
      peak_extraction_filter->SetToolkit(MaximaExtractionFilterType::MRTRIX);

    try{
      typedef mitk::ImageToItk< typename MaximaExtractionFilterType::CoefficientImageType > CasterType;
      typename CasterType::Pointer caster = CasterType::New();
      caster->SetInput(image);
      caster->Update();
      peak_extraction_filter->SetInput(caster->GetOutput());
    }
    catch(...)
    {
      std::cout << "wrong image type";
      return EXIT_FAILURE;
    }

    peak_extraction_filter->SetMaxNumPeaks(numPeaks);
    peak_extraction_filter->SetRelativePeakThreshold(rel_peakthr);
    peak_extraction_filter->SetAbsolutePeakThreshold(abs_peakthr);
    peak_extraction_filter->SetAngularThreshold(angular_thr);
    peak_extraction_filter->SetFlipX(flipX);
    peak_extraction_filter->SetFlipY(flipY);
    peak_extraction_filter->SetFlipZ(flipZ);
    peak_extraction_filter->SetScaleByGfa(scale_by_gfa);

    switch (normalization)
    {
    case 0:
      peak_extraction_filter->SetNormalizationMethod(MaximaExtractionFilterType::NO_NORM);
      break;
    case 1:
      peak_extraction_filter->SetNormalizationMethod(MaximaExtractionFilterType::MAX_VEC_NORM);
      break;
    case 2:
      peak_extraction_filter->SetNormalizationMethod(MaximaExtractionFilterType::SINGLE_VEC_NORM);
      break;
    }

    std::cout << "Starting extraction";
    peak_extraction_filter->Update();

    mitk::LocaleSwitch localeSwitch("C");
    // write direction image
    {
      typename MaximaExtractionFilterType::PeakImageType::Pointer itkImg = peak_extraction_filter->GetPeakImage();
      std::string outfilename = outRoot;
      outfilename.append("_PEAKS.nrrd");

      typedef itk::ImageFileWriter< typename MaximaExtractionFilterType::PeakImageType > WriterType;
      typename WriterType::Pointer writer = WriterType::New();
      writer->SetFileName(outfilename);
      writer->SetInput(itkImg);
      writer->Update();
    }

    // write num directions image
    {
      ItkUcharImgType::Pointer numDirImage = peak_extraction_filter->GetNumDirectionsImage();

      if (itkMaskImage.IsNotNull())
      {
        numDirImage->SetDirection(itkMaskImage->GetDirection());
        numDirImage->SetOrigin(itkMaskImage->GetOrigin());
      }

      std::string outfilename = outRoot.c_str();
      outfilename.append("_NUM_PEAKS.nrrd");
      typedef itk::ImageFileWriter< ItkUcharImgType > WriterType;
      WriterType::Pointer writer = WriterType::New();
      writer->SetFileName(outfilename);
      writer->SetInput(numDirImage);
      writer->Update();
    }
  }
  catch (itk::ExceptionObject e)
  {
    std::cout << e;
    return EXIT_FAILURE;
  }
  catch (std::exception e)
  {
    std::cout << e.what();
    return EXIT_FAILURE;
  }
  catch (...)
  {
    std::cout << "ERROR!?!";
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}

/*!
\brief Extract maxima in the input spherical harmonics image.
*/
int main(int argc, char* argv[])
{
  mitkCommandLineParser parser;
  parser.setArgumentPrefix("--", "-");
  parser.addArgument("image", "i", mitkCommandLineParser::InputFile, "Input image", "sh coefficient image", us::Any(), false);
  parser.addArgument("shOrder", "sh", mitkCommandLineParser::Int, "Spherical harmonics order", "spherical harmonics order");
  parser.addArgument("outroot", "o", mitkCommandLineParser::OutputDirectory, "Output directory", "output root", us::Any(), false);
  parser.addArgument("mask", "m", mitkCommandLineParser::InputFile, "Mask", "mask image");
  parser.addArgument("normalization", "n", mitkCommandLineParser::Int, "Normalization", "0=no norm, 1=max norm, 2=single vec norm", 1, true);
  parser.addArgument("numpeaks", "p", mitkCommandLineParser::Int, "Max. number of peaks", "maximum number of extracted peaks", 2, true);
  parser.addArgument("peakthres", "r", mitkCommandLineParser::Float, "Peak threshold", "peak threshold relative to largest peak", 0.4, true);
  parser.addArgument("abspeakthres", "a", mitkCommandLineParser::Float, "Absolute peak threshold", "absolute peak threshold weighted with local GFA value", 0.06, true);
  parser.addArgument("shConvention", "s", mitkCommandLineParser::String, "Use specified SH-basis", "use specified SH-basis (MITK, FSL, MRtrix)", std::string("MITK"), true);
  parser.addArgument("noFlip", "f", mitkCommandLineParser::Bool, "No flip", "do not flip input image to match MITK coordinate convention");

  parser.setCategory("Preprocessing Tools");
  parser.setTitle("Peak Extraction");
  parser.setDescription("Extract maxima in the input spherical harmonics image.");
  parser.setContributor("MIC");

  std::map<std::string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  int shOrder = -1;
  if (parsedArgs.count("shOrder"))
    shOrder = us::any_cast<int>(parsedArgs["shOrder"]);

  switch (shOrder)
  {
  case 4:
    return StartPeakExtraction<4>(argc, argv);
  case 6:
    return StartPeakExtraction<6>(argc, argv);
  case 8:
    return StartPeakExtraction<8>(argc, argv);
  case 10:
    return StartPeakExtraction<10>(argc, argv);
  case 12:
    return StartPeakExtraction<12>(argc, argv);
  }
  return EXIT_FAILURE;
}

max_stars_repo_name: wyyrepo/MITK

max_issues_repo_name: wyyrepo/MITK

max_forks_repo_name: wyyrepo/MITK


text:
/*
 *  Copyright (c) 2014-present, Facebook, Inc.
 *  All rights reserved.
 *
 *  This source code is licensed under the BSD-style license found in the
 *  LICENSE file in the root directory of this source tree. An additional grant
 *  of patent rights can be found in the PATENTS file in the same directory.
 *
 */

#include <iostream>

#include <boost/property_tree/ptree.hpp>

#include <gtest/gtest.h>

#include <osquery/core.h>
#include <osquery/distributed.h>
#include <osquery/enroll.h>
#include <osquery/sql.h>

#include "osquery/core/json.h"
#include "osquery/sql/sqlite_util.h"
#include "osquery/tests/test_additional_util.h"
#include "osquery/tests/test_util.h"

#include <rapidjson/prettywriter.h>

// distributed.cpp for why this is undefed
#undef GetObject

namespace pt = boost::property_tree;

DECLARE_string(distributed_tls_read_endpoint);
DECLARE_string(distributed_tls_write_endpoint);

namespace osquery {

class DistributedTests : public testing::Test {
 protected:
  void SetUp() {
    TLSServerRunner::start();
    TLSServerRunner::setClientConfig();
    clearNodeKey();

    distributed_tls_read_endpoint_ =
        Flag::getValue("distributed_tls_read_endpoint");
    Flag::updateValue("distributed_tls_read_endpoint", "/distributed_read");

    distributed_tls_write_endpoint_ =
        Flag::getValue("distributed_tls_write_endpoint");
    Flag::updateValue("distributed_tls_write_endpoint", "/distributed_write");

    Registry::get().setActive("distributed", "tls");
  }

  void TearDown() {
    TLSServerRunner::stop();
    TLSServerRunner::unsetClientConfig();
    clearNodeKey();

    Flag::updateValue("distributed_tls_read_endpoint",
                      distributed_tls_read_endpoint_);
    Flag::updateValue("distributed_tls_write_endpoint",
                      distributed_tls_write_endpoint_);
  }

 protected:
  std::string distributed_tls_read_endpoint_;
  std::string distributed_tls_write_endpoint_;
};

TEST_F(DistributedTests, test_serialize_distributed_query_request) {
  DistributedQueryRequest r;
  r.query = "foo";
  r.id = "bar";

  rapidjson::Document d(rapidjson::kObjectType);
  auto s = serializeDistributedQueryRequest(r, d);
  EXPECT_TRUE(s.ok());
  EXPECT_TRUE(d.HasMember("query") && d["query"].IsString());
  EXPECT_TRUE(d.HasMember("id") && d["id"].IsString());
  if (d.HasMember("query")) {
    EXPECT_EQ(std::string(d["query"].GetString()), "foo");
  }
  if (d.HasMember("id")) {
    EXPECT_EQ(std::string(d["id"].GetString()), "bar");
  }
}

TEST_F(DistributedTests, test_deserialize_distributed_query_request) {
  rapidjson::Document d(rapidjson::kObjectType);
  d.AddMember(rapidjson::Value("query", d.GetAllocator()).Move(),
              rapidjson::Value("foo", d.GetAllocator()),
              d.GetAllocator());

  d.AddMember(rapidjson::Value("id", d.GetAllocator()).Move(),
              rapidjson::Value("bar", d.GetAllocator()).Move(),
              d.GetAllocator());

  DistributedQueryRequest r;
  auto s = deserializeDistributedQueryRequest(d, r);
  EXPECT_TRUE(s.ok());
  EXPECT_EQ(r.query, "foo");
  EXPECT_EQ(r.id, "bar");
}

TEST_F(DistributedTests, test_deserialize_distributed_query_request_json) {
  auto json =
      "{"
      "  \"query\": \"foo\","
      "  \"id\": \"bar\""
      "}";

  DistributedQueryRequest r;
  auto s = deserializeDistributedQueryRequestJSON(json, r);
  EXPECT_TRUE(s.ok());
  EXPECT_EQ(r.query, "foo");
  EXPECT_EQ(r.id, "bar");
}

TEST_F(DistributedTests, test_serialize_distributed_query_result) {
  DistributedQueryResult r;
  r.request.query = "foo";
  r.request.id = "bar";

  Row r1;
  r1["foo"] = "bar";
  r.results = {r1};
  r.columns = {"foo"};
  rapidjson::Document d(rapidjson::kObjectType);
  auto s = serializeDistributedQueryResult(r, d);
  EXPECT_TRUE(s.ok());
  EXPECT_TRUE(d.IsObject());
  EXPECT_EQ(d["request"]["query"], "foo");
  EXPECT_EQ(d["request"]["id"], "bar");
  EXPECT_TRUE(d["results"].IsArray());
  for (const auto& q : d["results"].GetArray()) {
    for (const auto& row : q.GetObject()) {
      EXPECT_EQ(row.name, "foo");
      EXPECT_EQ(q[row.name], "bar");
    }
  }
}

TEST_F(DistributedTests, test_deserialize_distributed_query_result) {
  rapidjson::Document query_result(rapidjson::kObjectType);
  rapidjson::Document request(rapidjson::kObjectType);
  rapidjson::Value row(rapidjson::kObjectType);
  rapidjson::Document results(rapidjson::kArrayType);

  request.AddMember(
      rapidjson::Value("query", query_result.GetAllocator()).Move(),
      rapidjson::Value("bar", query_result.GetAllocator()),
      query_result.GetAllocator());

  request.AddMember(rapidjson::Value("id", query_result.GetAllocator()).Move(),
                    rapidjson::Value("foo", query_result.GetAllocator()).Move(),
                    query_result.GetAllocator());

  row.AddMember(rapidjson::Value("foo", query_result.GetAllocator()).Move(),
                rapidjson::Value("bar", query_result.GetAllocator()).Move(),
                query_result.GetAllocator());

  results.PushBack(rapidjson::Value(row, request.GetAllocator()).Move(),
                   request.GetAllocator());

  query_result.AddMember("request",
                         rapidjson::Value(request, query_result.GetAllocator()),
                         query_result.GetAllocator());
  query_result.AddMember("results",
                         rapidjson::Value(results, query_result.GetAllocator()),
                         query_result.GetAllocator());

  DistributedQueryResult r;
  auto s = deserializeDistributedQueryResult(query_result, r);
  EXPECT_EQ(r.request.id, "foo");
  EXPECT_EQ(r.request.query, "bar");
  EXPECT_EQ(r.results[0]["foo"], "bar");
}

TEST_F(DistributedTests, test_deserialize_distributed_query_result_json) {
  auto json =
      "{"
      "  \"request\": {"
      "    \"id\": \"foo\","
      "    \"query\": \"bar\""
      "  },"
      "  \"results\": ["
      "    {"
      "      \"foo\": \"bar\""
      "    }"
      "  ]"
      "}";

  DistributedQueryResult r;
  auto s = deserializeDistributedQueryResultJSON(json, r);
  EXPECT_TRUE(s.ok());
  EXPECT_EQ(r.request.id, "foo");
  EXPECT_EQ(r.request.query, "bar");
  EXPECT_EQ(r.results[0]["foo"], "bar");
}

TEST_F(DistributedTests, test_workflow) {
  auto dist = Distributed();
  auto s = dist.pullUpdates();
  EXPECT_TRUE(s.ok());
  EXPECT_EQ(s.toString(), "OK");

  EXPECT_EQ(dist.getPendingQueryCount(), 2U);
  EXPECT_EQ(dist.results_.size(), 0U);
  s = dist.runQueries();
  EXPECT_TRUE(s.ok());
  EXPECT_EQ(s.toString(), "OK");

  EXPECT_EQ(dist.getPendingQueryCount(), 0U);
  EXPECT_EQ(dist.results_.size(), 0U);
}
}

max_stars_repo_name: msekletar/osquery

max_issues_repo_name: msekletar/osquery


text:
//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_ParticleBank.hpp
//! \author Alex Robinson
//! \brief  Particle bank base class declaration
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_PARTICLE_BANK_HPP
#define MONTE_CARLO_PARTICLE_BANK_HPP

// Std Lib Includes
#include <memory>
#include <functional>

// Boost Includes
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/version.hpp>
#include <boost/serialization/shared_ptr.hpp>

// FRENSIE Includes
#include "MonteCarlo_ParticleState.hpp"
#include "MonteCarlo_NeutronState.hpp"
#include "Utility_ExplicitSerializationTemplateInstantiationMacros.hpp"
#include "Utility_List.hpp"

namespace MonteCarlo{

//! The particle bank base class (FIFO)
class ParticleBank
{

public:

  //! The compare function type
  typedef std::function<bool (const ParticleState&, const ParticleState&)> CompareFunctionType;

  //! Default Constructor
  ParticleBank();

  //! Destructor
  virtual ~ParticleBank()
  { /* ... */ }

  //! Check if the bank is empty
  bool isEmpty() const;

  //! The size of the bank
  unsigned long long size() const;

  //! Access the top element
  ParticleState& top();

  //! Access the top element
  const ParticleState& top() const;

  //! Insert a particle into the bank
  template<template<typename> class SmartPointer, typename State>
  void push( SmartPointer<State>& particle );

  //! Insert a particle into the bank (Most Efficient/Recommended)
  template<typename State>
  void push( std::shared_ptr<State>& particle );

  //! Insert a particle into the bank
  void push( const ParticleState& particle );

  //! Insert a neutron into the bank after an interaction
  template<template<typename> class SmartPointer>
  void push( SmartPointer<NeutronState>& neutron,
	     const int reaction );

  //! Insert a neutron into the bank after an interaction (Most Efficient/Recommended)
  virtual void push( std::shared_ptr<NeutronState>& neutron,
                     const int reaction );

  //! Insert a neutron into the bank after an interaction
  virtual void push( const NeutronState& neutron,
		     const int reaction );

  //! Pop the top particle from bank
  void pop();

  //! Pop the top particle from the bank and store it in the smart pointer
  template<template<typename> class SmartPointer>
  void pop( SmartPointer<ParticleState>& particle );

  //! Check if the bank is sorted
  virtual bool isSorted( const CompareFunctionType& compare_function );

  //! Sort the particle states
  virtual bool sort( const CompareFunctionType& compare_function );

  //! Merge the bank with another bank
  virtual void merge( ParticleBank& other_bank,
		      const CompareFunctionType& compare_function );

  //! Splice the bank with another bank
  virtual void splice( ParticleBank& other_bank );

protected:

  //! The bank container type
  typedef std::list<std::shared_ptr<ParticleState> > BankContainerType;

private:

  // Dereference a smart ptr
  static const ParticleState& dereference(
                               const std::shared_ptr<ParticleState>& pointer );

  // Save the bank to an archive
  template<typename Archive>
  void serialize( Archive& ar, const unsigned version )
  { ar & BOOST_SERIALIZATION_NVP(d_particle_states); }

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;

  // A list of particle states
  BankContainerType d_particle_states;
};

// Dereference a smart pointer
inline const ParticleState& ParticleBank::dereference(
                             const std::shared_ptr<ParticleState>& pointer )
{
  return *pointer;
}

} // end MonteCarlo namespace

BOOST_SERIALIZATION_CLASS_VERSION( ParticleBank, MonteCarlo, 0 );
EXTERN_EXPLICIT_CLASS_SERIALIZE_INST( MonteCarlo, ParticleBank );

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "MonteCarlo_ParticleBank_def.hpp"

//---------------------------------------------------------------------------//

#endif // end MonteCarlo_PARTICLE_BANK_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ParticleBank.hpp
//---------------------------------------------------------------------------//