<commit_before>
#include "physics/equipotential.hpp"

#include <vector>

#include "absl/strings/str_cat.h"
#include "base/not_null.hpp"
#include "geometry/barycentre_calculator.hpp"
#include "geometry/frame.hpp"
#include "geometry/grassmann.hpp"
#include "geometry/named_quantities.hpp"
#include "geometry/rotation.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "integrators/methods.hpp"
#include "integrators/embedded_explicit_runge_kutta_integrator.hpp"
#include "integrators/symmetric_linear_multistep_integrator.hpp"
#include "mathematica/mathematica.hpp"
#include "physics/body_centred_body_direction_dynamic_frame.hpp"
#include "physics/body_centred_non_rotating_dynamic_frame.hpp"
#include "physics/degrees_of_freedom.hpp"
#include "physics/dynamic_frame.hpp"
#include "physics/ephemeris.hpp"
#include "physics/solar_system.hpp"
#include "quantities/si.hpp"
#include "testing_utilities/solar_system_factory.hpp"

namespace principia {
namespace physics {

using base::make_not_null_unique;
using base::not_null;
using geometry::Arbitrary;
using geometry::Barycentre;
using geometry::Bivector;
using geometry::Frame;
using geometry::Inertial;
using geometry::Instant;
using geometry::Position;
using geometry::Rotation;
using geometry::Velocity;
using integrators::EmbeddedExplicitRungeKuttaIntegrator;
using integrators::SymmetricLinearMultistepIntegrator;
using integrators::methods::DormandPrince1986RK547FC;
using integrators::methods::QuinlanTremaine1990Order12;
using quantities::si::Day;
using quantities::si::Degree;
using quantities::si::Metre;
using quantities::si::Milli;
using quantities::si::Minute;
using quantities::si::Second;
using testing_utilities::SolarSystemFactory;

class EquipotentialTest : public ::testing::Test {
 protected:
  using Barycentric = Frame<enum class BarycentricTag, Inertial>;
  using World = Frame<enum class WorldTag, Arbitrary>;

  EquipotentialTest()
      : ephemeris_parameters_(
            SymmetricLinearMultistepIntegrator<QuinlanTremaine1990Order12,
                                               Position<Barycentric>>(),
            /*step=*/10 * Minute),
        solar_system_(make_not_null_unique<SolarSystem<Barycentric>>(
            SOLUTION_DIR / "astronomy" / "sol_gravity_model.proto.txt",
            SOLUTION_DIR / "astronomy" /
                "sol_initial_state_jd_2451545_000000000.proto.txt",
            /*ignore_frame=*/true)),
        ephemeris_(solar_system_->MakeEphemeris(
            /*accuracy_parameters=*/{/*fitting_tolerance=*/1 * Milli(Metre),
                                     /*geopotential_tolerance=*/0x1p-24},
            ephemeris_parameters_)),
        equipotential_parameters_(
            EmbeddedExplicitRungeKuttaIntegrator<
                DormandPrince1986RK547FC,
                Equipotential<Barycentric, World>::IndependentVariable,
                Position<World>,
                double>(),
            /*max_steps=*/1000,
            /*length_integration_tolerance=*/1 * Metre) {}

  Position<World> ComputePositionInWorld(
      Instant const& t,
      DynamicFrame<Barycentric, World> const& dynamic_frame,
      SolarSystemFactory::Index const body) {
    auto const to_this_frame = dynamic_frame.ToThisFrameAtTime(t);
    return to_this_frame.rigid_transformation()(
        solar_system_->trajectory(*ephemeris_, SolarSystemFactory::name(body))
            .EvaluatePosition(t));
  }

  // Logs to Mathematica the equipotential line for the given |body| in the
  // specified |dynamic_frame|.
  void LogEquipotentialLine(
      mathematica::Logger& logger,
      Bivector<double, World> const& plane,
      Instant const& t,
      DynamicFrame<Barycentric, World> const& dynamic_frame,
      SolarSystemFactory::Index const body,
      std::string_view const suffix = "") {
    Equipotential<Barycentric, World> const equipotential(
        equipotential_parameters_, &dynamic_frame);
    std::string const name = SolarSystemFactory::name(body);

    CHECK_OK(ephemeris_->Prolong(t));
    auto const& [positions, βs] = equipotential.ComputeLine(
        plane,
        t,
        ComputePositionInWorld(
            t0_, dynamic_frame, SolarSystemFactory::Mercury));
    logger.Set(absl::StrCat("equipotential", name, suffix),
               positions,
               mathematica::ExpressIn(Metre));
    logger.Set(absl::StrCat("beta", name, suffix), βs);
  }

  // Logs to Mathematica a family of equipotential lines determined by a
  // parameter.  There must exist an overload of |ComputeLine| with a
  // |LineParameter| as its third argument.
  template<typename LineParameter>
  void LogFamilyOfEquipotentialLines(
      mathematica::Logger& logger,
      DynamicFrame<Barycentric, World> const& dynamic_frame,
      int const number_of_days,
      std::string_view const suffix,
      std::function<std::vector<LineParameter>(
          Position<World> const& l4,
          Position<World> const& l5)> const& get_line_parameters) {
    Equipotential<Barycentric, World> const equipotential(
        equipotential_parameters_, &dynamic_frame);
    Bivector<double, World> const plane({0, 0, 1});

    std::vector<std::vector<std::vector<Position<World>>>> all_positions;
    std::vector<std::vector<std::vector<double>>> all_βs;
    for (int j = 0; j < number_of_days; ++j) {
      Instant const t = t0_ + j * Day;
      CHECK_OK(ephemeris_->Prolong(t));
      all_positions.emplace_back();
      all_βs.emplace_back();
      auto const earth_position =
          ComputePositionInWorld(t, dynamic_frame, SolarSystemFactory::Earth);
      auto const moon_position =
          ComputePositionInWorld(t, dynamic_frame, SolarSystemFactory::Moon);
      auto const moon_earth = earth_position - moon_position;

      Rotation<World, World> const rot_l4(-60 * Degree, plane);
      auto const moon_l4 = rot_l4(moon_earth);
      auto const l4 = moon_l4 + moon_position;
      Rotation<World, World> const rot_l5(60 * Degree, plane);
      auto const moon_l5 = rot_l5(moon_earth);
      auto const l5 = moon_l5 + moon_position;

      for (auto const& line_parameter : get_line_parameters(l4, l5)) {
        LOG(ERROR)<<line_parameter;
        auto const& [positions, βs] =
            equipotential.ComputeLine(plane, t, line_parameter);
        all_positions.back().push_back(positions);
        all_βs.back().push_back(βs);
      }
    }
    logger.Set(absl::StrCat("equipotentialsEarthMoon", suffix),
               all_positions,
               mathematica::ExpressIn(Metre));
    logger.Set(absl::StrCat("betasEarthMoon", suffix), all_βs);
  }

  Instant const t0_;
  Ephemeris<Barycentric>::FixedStepParameters const ephemeris_parameters_;
  not_null<std::unique_ptr<SolarSystem<Barycentric>>> const solar_system_;
  not_null<std::unique_ptr<Ephemeris<Barycentric>>> const ephemeris_;
  Equipotential<Barycentric, World>::AdaptiveParameters const
      equipotential_parameters_;
};

//#if !_DEBUG
TEST_F(EquipotentialTest, BodyCentredNonRotating) {
  mathematica::Logger logger(TEMP_DIR / "equipotential_bcnr.wl",
                             /*make_unique=*/false);
  auto const dynamic_frame(
      BodyCentredNonRotatingDynamicFrame<Barycentric, World>(
          ephemeris_.get(),
          solar_system_->massive_body(
              *ephemeris_, SolarSystemFactory::name(SolarSystemFactory::Sun))));
  Equipotential<Barycentric, World> const equipotential(
      equipotential_parameters_, &dynamic_frame);

  Bivector<double, World> const plane({2, 3, -5});

  LogEquipotentialLine(logger,
                       plane,
                       t0_ + 1 * Day,
                       dynamic_frame,
                       SolarSystemFactory::Mercury);
  LogEquipotentialLine(logger,
                       plane,
                       t0_ + 1 * Day,
                       dynamic_frame,
                       SolarSystemFactory::Earth);
  LogEquipotentialLine(logger,
                       plane,
                       t0_ + 1 * Day,
                       dynamic_frame,
                       SolarSystemFactory::Jupiter, "Close");
  LogEquipotentialLine(logger,
                       plane,
                       t0_ + 100 * Day,
                       dynamic_frame,
                       SolarSystemFactory::Jupiter, "Far");
}

TEST_F(EquipotentialTest, BodyCentredBodyDirection_EquidistantPoints) {
  mathematica::Logger logger(TEMP_DIR / "equipotential_bcbd.wl",
                             /*make_unique=*/true);
  auto const dynamic_frame(
      BodyCentredBodyDirectionDynamicFrame<Barycentric, World>(
          ephemeris_.get(),
          solar_system_->massive_body(
              *ephemeris_,
              SolarSystemFactory::name(SolarSystemFactory::Earth)),
          solar_system_->massive_body(
              *ephemeris_,
              SolarSystemFactory::name(SolarSystemFactory::Moon))));

  LogFamilyOfEquipotentialLines<Position<World>>(
      logger,
      dynamic_frame,
      /*number_of_days=*/30,
      /*suffix=*/"Positions",
      [](Position<World> const& l4, Position<World> const& l5) {
        std::vector<Position<World>> positions;
        for (int i = 0; i <= 10; ++i) {
          positions.push_back(Barycentre(
              std::pair{l4, l5}, std::pair{i / 10.0, (10.0 - i) / 10.0}));
        }
        return positions;
      });
}

TEST_F(EquipotentialTest, BodyCentredBodyDirection_EquidistantEnergies) {
  mathematica::Logger logger(TEMP_DIR / "equipotential_bcbd.wl",
                             /*make_unique=*/true);
  auto const dynamic_frame(
      BodyCentredBodyDirectionDynamicFrame<Barycentric, World>(
          ephemeris_.get(),
          solar_system_->massive_body(
              *ephemeris_,
              SolarSystemFactory::name(SolarSystemFactory::Earth)),
          solar_system_->massive_body(
              *ephemeris_,
              SolarSystemFactory::name(SolarSystemFactory::Moon))));

  LogFamilyOfEquipotentialLines<DegreesOfFreedom<World>>(
      logger,
      dynamic_frame,
      /*number_of_days=*/30,
      /*suffix=*/"Energies",
      [](Position<World> const& l4, Position<World> const& l5) {
        auto const midpoint = Barycentre(std::pair{l4, l5}, std::pair{1, 1});
        std::vector<DegreesOfFreedom<World>> degrees_of_freedom;
        for (int i = 0; i <= 10; ++i) {
          degrees_of_freedom.push_back(
              DegreesOfFreedom<World>(midpoint,
                                      Velocity<World>({i * 200 * Metre / Second,
                                                       0 * Metre / Second,
                                                       0 * Metre / Second})));
        }
        return degrees_of_freedom;
      });
}

//#endif

}  // namespace physics
}  // namespace principia
<commit_msg>A test with equidistant energies.<commit_after>
#include "physics/equipotential.hpp"

#include <vector>

#include "absl/strings/str_cat.h"
#include "base/not_null.hpp"
#include "geometry/barycentre_calculator.hpp"
#include "geometry/frame.hpp"
#include "geometry/grassmann.hpp"
#include "geometry/named_quantities.hpp"
#include "geometry/rotation.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "integrators/methods.hpp"
#include "integrators/embedded_explicit_runge_kutta_integrator.hpp"
#include "integrators/symmetric_linear_multistep_integrator.hpp"
#include "mathematica/mathematica.hpp"
#include "physics/body_centred_body_direction_dynamic_frame.hpp"
#include "physics/body_centred_non_rotating_dynamic_frame.hpp"
#include "physics/degrees_of_freedom.hpp"
#include "physics/dynamic_frame.hpp"
#include "physics/ephemeris.hpp"
#include "physics/solar_system.hpp"
#include "quantities/si.hpp"
#include "testing_utilities/solar_system_factory.hpp"

namespace principia {
namespace physics {

using base::make_not_null_unique;
using base::not_null;
using geometry::Arbitrary;
using geometry::Barycentre;
using geometry::Bivector;
using geometry::Frame;
using geometry::Inertial;
using geometry::Instant;
using geometry::Position;
using geometry::Rotation;
using geometry::Velocity;
using integrators::EmbeddedExplicitRungeKuttaIntegrator;
using integrators::SymmetricLinearMultistepIntegrator;
using integrators::methods::DormandPrince1986RK547FC;
using integrators::methods::QuinlanTremaine1990Order12;
using quantities::si::Day;
using quantities::si::Degree;
using quantities::si::Metre;
using quantities::si::Milli;
using quantities::si::Minute;
using quantities::si::Second;
using testing_utilities::SolarSystemFactory;

class EquipotentialTest : public ::testing::Test {
 protected:
  using Barycentric = Frame<enum class BarycentricTag, Inertial>;
  using World = Frame<enum class WorldTag, Arbitrary>;

  EquipotentialTest()
      : ephemeris_parameters_(
            SymmetricLinearMultistepIntegrator<QuinlanTremaine1990Order12,
                                               Position<Barycentric>>(),
            /*step=*/10 * Minute),
        solar_system_(make_not_null_unique<SolarSystem<Barycentric>>(
            SOLUTION_DIR / "astronomy" / "sol_gravity_model.proto.txt",
            SOLUTION_DIR / "astronomy" /
                "sol_initial_state_jd_2451545_000000000.proto.txt",
            /*ignore_frame=*/true)),
        ephemeris_(solar_system_->MakeEphemeris(
            /*accuracy_parameters=*/{/*fitting_tolerance=*/1 * Milli(Metre),
                                     /*geopotential_tolerance=*/0x1p-24},
            ephemeris_parameters_)),
        equipotential_parameters_(
            EmbeddedExplicitRungeKuttaIntegrator<
                DormandPrince1986RK547FC,
                Equipotential<Barycentric, World>::IndependentVariable,
                Position<World>,
                double>(),
            /*max_steps=*/1000,
            /*length_integration_tolerance=*/1 * Metre) {}

  Position<World> ComputePositionInWorld(
      Instant const& t,
      DynamicFrame<Barycentric, World> const& dynamic_frame,
      SolarSystemFactory::Index const body) {
    auto const to_this_frame = dynamic_frame.ToThisFrameAtTime(t);
    return to_this_frame.rigid_transformation()(
        solar_system_->trajectory(*ephemeris_, SolarSystemFactory::name(body))
            .EvaluatePosition(t));
  }

  // Logs to Mathematica the equipotential line for the given |body| in the
  // specified |dynamic_frame|.
  void LogEquipotentialLine(
      mathematica::Logger& logger,
      Bivector<double, World> const& plane,
      Instant const& t,
      DynamicFrame<Barycentric, World> const& dynamic_frame,
      SolarSystemFactory::Index const body,
      std::string_view const suffix = "") {
    Equipotential<Barycentric, World> const equipotential(
        equipotential_parameters_, &dynamic_frame);
    std::string const name = SolarSystemFactory::name(body);

    CHECK_OK(ephemeris_->Prolong(t));
    auto const& [positions, βs] = equipotential.ComputeLine(
        plane,
        t,
        ComputePositionInWorld(
            t0_, dynamic_frame, SolarSystemFactory::Mercury));
    logger.Set(absl::StrCat("equipotential", name, suffix),
               positions,
               mathematica::ExpressIn(Metre));
    logger.Set(absl::StrCat("beta", name, suffix), βs);
  }

  // Logs to Mathematica a family of equipotential lines determined by a
  // parameter.  There must exist an overload of |ComputeLine| with a
  // |LineParameter| as its third argument.
  template<typename LineParameter>
  void LogFamilyOfEquipotentialLines(
      mathematica::Logger& logger,
      DynamicFrame<Barycentric, World> const& dynamic_frame,
      int const number_of_days,
      std::string_view const suffix,
      std::function<std::vector<LineParameter>(
          Position<World> const& l4,
          Position<World> const& l5)> const& get_line_parameters) {
    Equipotential<Barycentric, World> const equipotential(
        equipotential_parameters_, &dynamic_frame);
    Bivector<double, World> const plane({0, 0, 1});

    std::vector<std::vector<std::vector<Position<World>>>> all_positions;
    std::vector<std::vector<std::vector<double>>> all_βs;
    for (int j = 0; j < number_of_days; ++j) {
      Instant const t = t0_ + j * Day;
      CHECK_OK(ephemeris_->Prolong(t));
      all_positions.emplace_back();
      all_βs.emplace_back();
      auto const earth_position =
          ComputePositionInWorld(t, dynamic_frame, SolarSystemFactory::Earth);
      auto const moon_position =
          ComputePositionInWorld(t, dynamic_frame, SolarSystemFactory::Moon);
      auto const moon_earth = earth_position - moon_position;

      Rotation<World, World> const rot_l4(-60 * Degree, plane);
      auto const moon_l4 = rot_l4(moon_earth);
      auto const l4 = moon_l4 + moon_position;
      Rotation<World, World> const rot_l5(60 * Degree, plane);
      auto const moon_l5 = rot_l5(moon_earth);
      auto const l5 = moon_l5 + moon_position;

      for (auto const& line_parameter : get_line_parameters(l4, l5)) {
        auto const& [positions, βs] =
            equipotential.ComputeLine(plane, t, line_parameter);
        all_positions.back().push_back(positions);
        all_βs.back().push_back(βs);
      }
    }
    logger.Set(absl::StrCat("equipotentialsEarthMoon", suffix),
               all_positions,
               mathematica::ExpressIn(Metre));
    logger.Set(absl::StrCat("betasEarthMoon", suffix), all_βs);
  }

  Instant const t0_;
  Ephemeris<Barycentric>::FixedStepParameters const ephemeris_parameters_;
  not_null<std::unique_ptr<SolarSystem<Barycentric>>> const solar_system_;
  not_null<std::unique_ptr<Ephemeris<Barycentric>>> const ephemeris_;
  Equipotential<Barycentric, World>::AdaptiveParameters const
      equipotential_parameters_;
};

#if !_DEBUG
TEST_F(EquipotentialTest, BodyCentredNonRotating) {
  mathematica::Logger logger(TEMP_DIR / "equipotential_bcnr.wl",
                             /*make_unique=*/false);
  auto const dynamic_frame(
      BodyCentredNonRotatingDynamicFrame<Barycentric, World>(
          ephemeris_.get(),
          solar_system_->massive_body(
              *ephemeris_, SolarSystemFactory::name(SolarSystemFactory::Sun))));
  Equipotential<Barycentric, World> const equipotential(
      equipotential_parameters_, &dynamic_frame);

  Bivector<double, World> const plane({2, 3, -5});

  LogEquipotentialLine(logger,
                       plane,
                       t0_ + 1 * Day,
                       dynamic_frame,
                       SolarSystemFactory::Mercury);
  LogEquipotentialLine(logger,
                       plane,
                       t0_ + 1 * Day,
                       dynamic_frame,
                       SolarSystemFactory::Earth);
  LogEquipotentialLine(logger,
                       plane,
                       t0_ + 1 * Day,
                       dynamic_frame,
                       SolarSystemFactory::Jupiter, "Close");
  LogEquipotentialLine(logger,
                       plane,
                       t0_ + 100 * Day,
                       dynamic_frame,
                       SolarSystemFactory::Jupiter, "Far");
}

TEST_F(EquipotentialTest, BodyCentredBodyDirection_EquidistantPoints) {
  mathematica::Logger logger(TEMP_DIR / "equipotential_bcbd.wl",
                             /*make_unique=*/true);
  auto const dynamic_frame(
      BodyCentredBodyDirectionDynamicFrame<Barycentric, World>(
          ephemeris_.get(),
          solar_system_->massive_body(
              *ephemeris_,
              SolarSystemFactory::name(SolarSystemFactory::Earth)),
          solar_system_->massive_body(
              *ephemeris_,
              SolarSystemFactory::name(SolarSystemFactory::Moon))));

  LogFamilyOfEquipotentialLines<Position<World>>(
      logger,
      dynamic_frame,
      /*number_of_days=*/30,
      /*suffix=*/"Positions",
      [](Position<World> const& l4, Position<World> const& l5) {
        std::vector<Position<World>> positions;
        for (int i = 0; i <= 10; ++i) {
          positions.push_back(Barycentre(
              std::pair{l4, l5}, std::pair{i / 10.0, (10.0 - i) / 10.0}));
        }
        return positions;
      });
}

TEST_F(EquipotentialTest, BodyCentredBodyDirection_EquidistantEnergies) {
  mathematica::Logger logger(TEMP_DIR / "equipotential_bcbd.wl",
                             /*make_unique=*/true);
  auto const dynamic_frame(
      BodyCentredBodyDirectionDynamicFrame<Barycentric, World>(
          ephemeris_.get(),
          solar_system_->massive_body(
              *ephemeris_,
              SolarSystemFactory::name(SolarSystemFactory::Earth)),
          solar_system_->massive_body(
              *ephemeris_,
              SolarSystemFactory::name(SolarSystemFactory::Moon))));

  LogFamilyOfEquipotentialLines<DegreesOfFreedom<World>>(
      logger,
      dynamic_frame,
      /*number_of_days=*/30,
      /*suffix=*/"Energies",
      [](Position<World> const& l4, Position<World> const& l5) {
        auto const midpoint = Barycentre(std::pair{l4, l5}, std::pair{1, 1});
        std::vector<DegreesOfFreedom<World>> degrees_of_freedom;
        for (int i = 0; i <= 10; ++i) {
          degrees_of_freedom.push_back(
              DegreesOfFreedom<World>(midpoint,
                                      Velocity<World>({i * 100 * Metre / Second,
                                                       0 * Metre / Second,
                                                       0 * Metre / Second})));
        }
        for (int i = 0; i <= 10; ++i) {
          degrees_of_freedom.push_back(DegreesOfFreedom<World>(
              midpoint,
              Velocity<World>({(1100 + i * 10) * Metre / Second,
                               0 * Metre / Second,
                               0 * Metre / Second})));
        }
        return degrees_of_freedom;
      });
}

#endif

}  // namespace physics
}  // namespace principia
<|endoftext|>


<commit_before>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  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 Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Jon Binney */

#include <moveit/occupancy_map_monitor/point_cloud_occupancy_map_updater.h>
#include <tf/tf.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/ros/conversions.h>
#include <moveit/robot_self_filter/self_mask.h>

namespace occupancy_map_monitor
{

PointCloudOccupancyMapUpdater::PointCloudOccupancyMapUpdater(const boost::shared_ptr<tf::Transformer> &tf, const std::string &map_frame)
  : tf_(tf), map_frame_(map_frame),
    point_cloud_subscriber_(NULL),
    point_cloud_filter_(NULL)
{
}

PointCloudOccupancyMapUpdater::~PointCloudOccupancyMapUpdater(void)
{
  delete point_cloud_filter_;
  delete point_cloud_subscriber_;
}

bool PointCloudOccupancyMapUpdater::setParams(XmlRpc::XmlRpcValue &params)
{
  if(!params.hasMember("point_cloud_topic"))
    return false;
  std::string point_cloud_topic = std::string (params["point_cloud_topic"]);

  if(!params.hasMember("max_range"))
    return false;
  double max_range = double (params["max_range"]);

  if(!params.hasMember("frame_subsample"))
    return false;
  size_t frame_subsample = int (params["frame_subsample"]);

  if(!params.hasMember("point_subsample"))
    return false;
  size_t point_subsample = int (params["point_subsample"]);

  std::vector<robot_self_filter::LinkInfo> links;
  if(params.hasMember("self_mask"))
  {
    ROS_INFO("Configuring self mask");
    if(!robot_self_filter::createLinksFromParams(params["self_mask"], links))
    {
      ROS_ERROR("Failed to load self mask");
      return false;
    }
  }

  return this->setParams(point_cloud_topic, max_range, frame_subsample, point_subsample, links);
}

bool PointCloudOccupancyMapUpdater::setParams(const std::string &point_cloud_topic, double max_range,  size_t frame_subsample,
                                              size_t point_subsample, const std::vector<robot_self_filter::LinkInfo> links)
{
  point_cloud_topic_ = point_cloud_topic;
  max_range_ = max_range;
  frame_subsample_ = frame_subsample;
  point_subsample_ = point_subsample;
  if (tf_)
    self_mask_.reset(new robot_self_filter::SelfMask(*tf_, links));
  return true;
}

void PointCloudOccupancyMapUpdater::initialize()
{
  /* subscribe to point cloud topic using tf filter*/
  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(root_nh_, point_cloud_topic_, 1024);
  if (tf_)
  {
    point_cloud_filter_ = new tf::MessageFilter<sensor_msgs::PointCloud2>(*point_cloud_subscriber_, *tf_, map_frame_, 1024);
    point_cloud_filter_->registerCallback(boost::bind(&PointCloudOccupancyMapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), point_cloud_filter_->getTargetFramesString().c_str());
  }
  else
  {
    point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOccupancyMapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str());
  }
}

void PointCloudOccupancyMapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
{
  ROS_DEBUG("Got a point cloud message");
  {
    boost::lock_guard<boost::mutex> _lock(last_point_cloud_mutex_);
    last_point_cloud_ = cloud_msg;
  }
  /* tell the monitor that we are ready to update the map */
  notifyUpdateReady();
}

void PointCloudOccupancyMapUpdater::process(const OccMapTreePtr &tree)
{
  ROS_DEBUG("Updating occupancy map with new cloud");
  sensor_msgs::PointCloud2::ConstPtr cloud;
  {
    boost::lock_guard<boost::mutex> _lock(last_point_cloud_mutex_);
    cloud = last_point_cloud_;
    last_point_cloud_.reset();
  }

  if (cloud)
  {
    processCloud(tree, cloud);
    ROS_DEBUG("Done updating occupancy map");
  }
  else
    ROS_DEBUG("No point cloud to process");
}

void PointCloudOccupancyMapUpdater::processCloud(const OccMapTreePtr &tree, const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
{
  if (!tf_)
    return;

  /* get transform for cloud into map frame */
  tf::StampedTransform map_H_sensor;
  try
  {
    tf_->lookupTransform(map_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp, map_H_sensor);
  }
  catch (tf::TransformException& ex)
  {
    ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback");
    return;
  }

  /* convert cloud message to pcl cloud object */
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(*cloud_msg, cloud);

  /* compute sensor origin in map frame */
  tf::Vector3 sensor_origin_tf = map_H_sensor.getOrigin();
  octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());

  /* mask out points on the robot */
  std::vector<int> mask;
  if(self_mask_)
    self_mask_->maskContainment(cloud, mask);

  /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
   * should be occupied */
  octomap::KeySet free_cells, occupied_cells;
  unsigned int row, col;
  for(row = 0; row < cloud.height; row += point_subsample_)
  {
    for(col = 0; col < cloud.width; col += point_subsample_)
    {
      bool self_point = false;
      if(!mask.empty() && mask[row*cloud.width + col] == robot_self_filter::INSIDE)
        self_point = true;

      pcl::PointXYZ p = cloud(col, row);

      /* check for NaN */
      if(!((p.x == p.x) && (p.y == p.y) && (p.z == p.z)))
        continue;

      /* transform to map frame */
      tf::Vector3 point_tf = map_H_sensor * tf::Vector3(p.x, p.y, p.z);
      octomap::point3d point(point_tf.getX(), point_tf.getY(), point_tf.getZ());

      /* free cells along ray */
      if (tree->computeRayKeys(sensor_origin, point, key_ray_))
        free_cells.insert(key_ray_.begin(), key_ray_.end());

      /* occupied cell at ray endpoint if ray is shorter than max range and this point
         isn't on a part of the robot*/
      if(!self_point)
      {
        double range = (point - sensor_origin).norm();
        if(range < max_range_)
        {
          octomap::OcTreeKey key;
          if (tree->genKey(point, key))
            occupied_cells.insert(key);
        }
      }
    }
  }

  ROS_DEBUG("Marking free cells in octomap");

  /* mark free cells only if not seen occupied in this cloud */
  for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it)
    /* this check seems unnecessary since we would just overwrite them in the next loop? -jbinney */
    if (occupied_cells.find(*it) == occupied_cells.end())
      tree->updateNode(*it, false);

  ROS_DEBUG("Marking occupied cells in octomap");

  /* now mark all occupied cells */
  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = free_cells.end(); it != end; it++)
    tree->updateNode(*it, true);
}

}
<commit_msg>fixed use of deprecated octomap function<commit_after>/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  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 Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Jon Binney */

#include <moveit/occupancy_map_monitor/point_cloud_occupancy_map_updater.h>
#include <tf/tf.h>
#include <tf/message_filter.h>
#include <message_filters/subscriber.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/ros/conversions.h>
#include <moveit/robot_self_filter/self_mask.h>

namespace occupancy_map_monitor
{

PointCloudOccupancyMapUpdater::PointCloudOccupancyMapUpdater(const boost::shared_ptr<tf::Transformer> &tf, const std::string &map_frame)
  : tf_(tf), map_frame_(map_frame),
    point_cloud_subscriber_(NULL),
    point_cloud_filter_(NULL)
{
}

PointCloudOccupancyMapUpdater::~PointCloudOccupancyMapUpdater(void)
{
  delete point_cloud_filter_;
  delete point_cloud_subscriber_;
}

bool PointCloudOccupancyMapUpdater::setParams(XmlRpc::XmlRpcValue &params)
{
  if(!params.hasMember("point_cloud_topic"))
    return false;
  std::string point_cloud_topic = std::string (params["point_cloud_topic"]);

  if(!params.hasMember("max_range"))
    return false;
  double max_range = double (params["max_range"]);

  if(!params.hasMember("frame_subsample"))
    return false;
  size_t frame_subsample = int (params["frame_subsample"]);

  if(!params.hasMember("point_subsample"))
    return false;
  size_t point_subsample = int (params["point_subsample"]);

  std::vector<robot_self_filter::LinkInfo> links;
  if(params.hasMember("self_mask"))
  {
    ROS_INFO("Configuring self mask");
    if(!robot_self_filter::createLinksFromParams(params["self_mask"], links))
    {
      ROS_ERROR("Failed to load self mask");
      return false;
    }
  }

  return this->setParams(point_cloud_topic, max_range, frame_subsample, point_subsample, links);
}

bool PointCloudOccupancyMapUpdater::setParams(const std::string &point_cloud_topic, double max_range,  size_t frame_subsample,
                                              size_t point_subsample, const std::vector<robot_self_filter::LinkInfo> links)
{
  point_cloud_topic_ = point_cloud_topic;
  max_range_ = max_range;
  frame_subsample_ = frame_subsample;
  point_subsample_ = point_subsample;
  if (tf_)
    self_mask_.reset(new robot_self_filter::SelfMask(*tf_, links));
  return true;
}

void PointCloudOccupancyMapUpdater::initialize()
{
  /* subscribe to point cloud topic using tf filter*/
  point_cloud_subscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud2>(root_nh_, point_cloud_topic_, 1024);
  if (tf_)
  {
    point_cloud_filter_ = new tf::MessageFilter<sensor_msgs::PointCloud2>(*point_cloud_subscriber_, *tf_, map_frame_, 1024);
    point_cloud_filter_->registerCallback(boost::bind(&PointCloudOccupancyMapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO("Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), point_cloud_filter_->getTargetFramesString().c_str());
  }
  else
  {
    point_cloud_subscriber_->registerCallback(boost::bind(&PointCloudOccupancyMapUpdater::cloudMsgCallback, this, _1));
    ROS_INFO("Listening to '%s'", point_cloud_topic_.c_str());
  }
}

void PointCloudOccupancyMapUpdater::cloudMsgCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
{
  ROS_DEBUG("Got a point cloud message");
  {
    boost::lock_guard<boost::mutex> _lock(last_point_cloud_mutex_);
    last_point_cloud_ = cloud_msg;
  }
  /* tell the monitor that we are ready to update the map */
  notifyUpdateReady();
}

void PointCloudOccupancyMapUpdater::process(const OccMapTreePtr &tree)
{
  ROS_DEBUG("Updating occupancy map with new cloud");
  sensor_msgs::PointCloud2::ConstPtr cloud;
  {
    boost::lock_guard<boost::mutex> _lock(last_point_cloud_mutex_);
    cloud = last_point_cloud_;
    last_point_cloud_.reset();
  }

  if (cloud)
  {
    processCloud(tree, cloud);
    ROS_DEBUG("Done updating occupancy map");
  }
  else
    ROS_DEBUG("No point cloud to process");
}

void PointCloudOccupancyMapUpdater::processCloud(const OccMapTreePtr &tree, const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
{
  if (!tf_)
    return;

  /* get transform for cloud into map frame */
  tf::StampedTransform map_H_sensor;
  try
  {
    tf_->lookupTransform(map_frame_, cloud_msg->header.frame_id, cloud_msg->header.stamp, map_H_sensor);
  }
  catch (tf::TransformException& ex)
  {
    ROS_ERROR_STREAM("Transform error of sensor data: " << ex.what() << ", quitting callback");
    return;
  }

  /* convert cloud message to pcl cloud object */
  pcl::PointCloud<pcl::PointXYZ> cloud;
  pcl::fromROSMsg(*cloud_msg, cloud);

  /* compute sensor origin in map frame */
  tf::Vector3 sensor_origin_tf = map_H_sensor.getOrigin();
  octomap::point3d sensor_origin(sensor_origin_tf.getX(), sensor_origin_tf.getY(), sensor_origin_tf.getZ());

  /* mask out points on the robot */
  std::vector<int> mask;
  if(self_mask_)
    self_mask_->maskContainment(cloud, mask);

  /* do ray tracing to find which cells this point cloud indicates should be free, and which it indicates
   * should be occupied */
  octomap::KeySet free_cells, occupied_cells;
  unsigned int row, col;
  for(row = 0; row < cloud.height; row += point_subsample_)
  {
    for(col = 0; col < cloud.width; col += point_subsample_)
    {
      bool self_point = false;
      if(!mask.empty() && mask[row*cloud.width + col] == robot_self_filter::INSIDE)
        self_point = true;

      pcl::PointXYZ p = cloud(col, row);

      /* check for NaN */
      if(!((p.x == p.x) && (p.y == p.y) && (p.z == p.z)))
        continue;

      /* transform to map frame */
      tf::Vector3 point_tf = map_H_sensor * tf::Vector3(p.x, p.y, p.z);
      octomap::point3d point(point_tf.getX(), point_tf.getY(), point_tf.getZ());

      /* free cells along ray */
      if (tree->computeRayKeys(sensor_origin, point, key_ray_))
        free_cells.insert(key_ray_.begin(), key_ray_.end());

      /* occupied cell at ray endpoint if ray is shorter than max range and this point
         isn't on a part of the robot*/
      if(!self_point)
      {
        double range = (point - sensor_origin).norm();
        if(range < max_range_)
        {
          octomap::OcTreeKey key;
          if (tree->coordToKeyChecked(point, key))
            occupied_cells.insert(key);
        }
      }
    }
  }

  ROS_DEBUG("Marking free cells in octomap");

  /* mark free cells only if not seen occupied in this cloud */
  for (octomap::KeySet::iterator it = free_cells.begin(), end = free_cells.end(); it != end; ++it)
    /* this check seems unnecessary since we would just overwrite them in the next loop? -jbinney */
    if (occupied_cells.find(*it) == occupied_cells.end())
      tree->updateNode(*it, false);

  ROS_DEBUG("Marking occupied cells in octomap");

  /* now mark all occupied cells */
  for (octomap::KeySet::iterator it = occupied_cells.begin(), end = free_cells.end(); it != end; it++)
    tree->updateNode(*it, true);
}

}
<|endoftext|>


<commit_before>
// Copyright (c) 2015 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "openMVG/matching_image_collection/Cascade_Hashing_Matcher_Regions_AllInMemory.hpp"
#include "openMVG/matching/matcher_cascade_hashing.hpp"
#include "openMVG/matching/indMatchDecoratorXY.hpp"
#include "openMVG/matching/matching_filters.hpp"
#include <openMVG/config.hpp>

#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"
#include "third_party/progress/progress.hpp"

namespace openMVG {
namespace matching_image_collection {

using namespace openMVG::matching;
using namespace openMVG::features;

ImageCollectionMatcher_CascadeHashing
::ImageCollectionMatcher_CascadeHashing
(
  float distRatio
):IImageCollectionMatcher(), f_dist_ratio_(distRatio)
{
}

namespace impl
{
template <typename ScalarT>
void Match
(
  const sfm::SfM_Data & sfm_data,
  const features::RegionsPerView& regionsPerView,
  const Pair_Set & pairs,
  EImageDescriberType descType,
  float fDistRatio,
  PairwiseMatches & map_PutativesMatches // the pairwise photometric corresponding points
)
{
  C_Progress_display my_progress_bar( pairs.size() );

  // Collect used view indexes
  std::set<IndexT> used_index;
  // Sort pairs according the first index to minimize later memory swapping
  typedef std::map<IndexT, std::vector<IndexT> > Map_vectorT;
  Map_vectorT map_Pairs;
  for (Pair_Set::const_iterator iter = pairs.begin(); iter != pairs.end(); ++iter)
  {
    map_Pairs[iter->first].push_back(iter->second);
    used_index.insert(iter->first);
    used_index.insert(iter->second);
  }

  typedef Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> BaseMat;

  // Init the cascade hasher
  CascadeHasher cascade_hasher;
  if (!used_index.empty())
  {
    const IndexT I = *used_index.begin();
    const features::Regions &regionsI = regionsPerView.getRegions(I, descType);
    const size_t dimension = regionsI.DescriptorLength();
    cascade_hasher.Init(dimension);
  }

  std::map<IndexT, HashedDescriptions> hashed_base_;

  // Compute the zero mean descriptor that will be used for hashing (one for all the image regions)
  Eigen::VectorXf zero_mean_descriptor;
  {
    Eigen::MatrixXf matForZeroMean;
    for (int i =0; i < used_index.size(); ++i)
    {
      std::set<IndexT>::const_iterator iter = used_index.begin();
      std::advance(iter, i);
      const IndexT I = *iter;
      const features::Regions &regionsI = regionsPerView.getRegions(I, descType);
      const ScalarT * tabI =
        reinterpret_cast<const ScalarT*>(regionsI.DescriptorRawData());
      const size_t dimension = regionsI.DescriptorLength();
      if (i==0)
      {
        matForZeroMean.resize(used_index.size(), dimension);
        matForZeroMean.fill(0.0f);
      }
      if (regionsI.RegionCount() > 0)
      {
        Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.RegionCount(), dimension);
        matForZeroMean.row(i) = CascadeHasher::GetZeroMeanDescriptor(mat_I);
      }
    }
    zero_mean_descriptor = CascadeHasher::GetZeroMeanDescriptor(matForZeroMean);
  }

  // Index the input regions
  #pragma omp parallel for schedule(dynamic)
  for (int i =0; i < used_index.size(); ++i)
  {
    std::set<IndexT>::const_iterator iter = used_index.begin();
    std::advance(iter, i);
    const IndexT I = *iter;
    const features::Regions &regionsI = regionsPerView.getRegions(I, descType);
    const ScalarT * tabI =
      reinterpret_cast<const ScalarT*>(regionsI.DescriptorRawData());
    const size_t dimension = regionsI.DescriptorLength();

    Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.RegionCount(), dimension);
    HashedDescriptions hashed_description = cascade_hasher.CreateHashedDescriptions(mat_I,
      zero_mean_descriptor);
    #pragma omp critical
    {
      hashed_base_[I] = std::move(hashed_description);
    }
  }

  // Perform matching between all the pairs
  for (Map_vectorT::const_iterator iter = map_Pairs.begin();
    iter != map_Pairs.end(); ++iter)
  {
    const IndexT I = iter->first;
    const std::vector<IndexT> & indexToCompare = iter->second;

    const features::Regions &regionsI = regionsPerView.getRegions(I, descType);
    if (regionsI.RegionCount() == 0)
    {
      my_progress_bar += indexToCompare.size();
      continue;
    }

    const std::vector<features::PointFeature> pointFeaturesI = regionsI.GetRegionsPositions();
    const ScalarT * tabI =
      reinterpret_cast<const ScalarT*>(regionsI.DescriptorRawData());
    const size_t dimension = regionsI.DescriptorLength();
    Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.RegionCount(), dimension);
    #pragma omp parallel for schedule(dynamic)
    for (int j = 0; j < (int)indexToCompare.size(); ++j)
    {
      size_t J = indexToCompare[j];
      const features::Regions &regionsJ = regionsPerView.getRegions(I, descType);

      if (!regionsPerView.viewExist(J)
          || regionsI.Type_id() != regionsJ.Type_id())
      {
        #pragma omp critical
        ++my_progress_bar;
        continue;
      }

      // Matrix representation of the query input data;
      const ScalarT * tabJ = reinterpret_cast<const ScalarT*>(regionsJ.DescriptorRawData());
      Eigen::Map<BaseMat> mat_J( (ScalarT*)tabJ, regionsJ.RegionCount(), dimension);

      IndMatches pvec_indices;
      typedef typename Accumulator<ScalarT>::Type ResultType;
      std::vector<ResultType> pvec_distances;
      pvec_distances.reserve(regionsJ.RegionCount() * 2);
      pvec_indices.reserve(regionsJ.RegionCount() * 2);

      // Match the query descriptors to the database
      cascade_hasher.Match_HashedDescriptions<BaseMat, ResultType>(
        hashed_base_[J], mat_J,
        hashed_base_[I], mat_I,
        &pvec_indices, &pvec_distances);

      std::vector<int> vec_nn_ratio_idx;
      // Filter the matches using a distance ratio test:
      //   The probability that a match is correct is determined by taking
      //   the ratio of distance from the closest neighbor to the distance
      //   of the second closest.
      matching::NNdistanceRatio(
        pvec_distances.begin(), // distance start
        pvec_distances.end(),   // distance end
        2, // Number of neighbor in iterator sequence (minimum required 2)
        vec_nn_ratio_idx, // output (indices that respect the distance Ratio)
        Square(fDistRatio));

      matching::IndMatches vec_putative_matches;
      vec_putative_matches.reserve(vec_nn_ratio_idx.size());
      for (size_t k=0; k < vec_nn_ratio_idx.size(); ++k)
      {
        const size_t index = vec_nn_ratio_idx[k];
        vec_putative_matches.emplace_back(pvec_indices[index*2]._j, pvec_indices[index*2]._i);
      }

      // Remove duplicates
      matching::IndMatch::getDeduplicated(vec_putative_matches);

      // Remove matches that have the same (X,Y) coordinates
      const std::vector<features::PointFeature> pointFeaturesJ = regionsJ.GetRegionsPositions();
      matching::IndMatchDecorator<float> matchDeduplicator(vec_putative_matches,
        pointFeaturesI, pointFeaturesJ);
      matchDeduplicator.getDeduplicated(vec_putative_matches);

      #pragma omp critical
      {
        ++my_progress_bar;
        if (!vec_putative_matches.empty())
        {
          map_PutativesMatches[std::make_pair(I,J)].emplace(descType, std::move(vec_putative_matches));
        }
      }
    }
  }
}
} // namespace impl

void ImageCollectionMatcher_CascadeHashing::Match
(
  const sfm::SfM_Data & sfm_data,
  const features::RegionsPerView& regionsPerView,
  const Pair_Set & pairs,
  features::EImageDescriberType descType,
  PairwiseMatches & map_PutativesMatches // the pairwise photometric corresponding points
) const
{
#if OPENMVG_IS_DEFINED(OPENMVG_USE_OPENMP)
  OPENMVG_LOG_DEBUG("Using the OPENMP thread interface");
#endif

  if (regionsPerView.isEmpty())
    return;

  const features::Regions& regions = regionsPerView.getFirstViewRegions(descType);

  if (regions.IsBinary())
    return;

  if(regions.Type_id() == typeid(unsigned char).name())
  {
    impl::Match<unsigned char>(
      sfm_data,
      regionsPerView,
      pairs,
      descType,
      f_dist_ratio_,
      map_PutativesMatches);
  }
  else
  if(regions.Type_id() == typeid(float).name())
  {
    impl::Match<float>(
      sfm_data,
      regionsPerView,
      pairs,
      descType,
      f_dist_ratio_,
      map_PutativesMatches);
  }
  else
  {
    OPENMVG_LOG_WARNING("Matcher not implemented for this region type");
  }
}

} // namespace openMVG
} // namespace matching_image_collection
<commit_msg>[matching_image_collection] Add safety assert<commit_after>
// Copyright (c) 2015 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "openMVG/matching_image_collection/Cascade_Hashing_Matcher_Regions_AllInMemory.hpp"
#include "openMVG/matching/matcher_cascade_hashing.hpp"
#include "openMVG/matching/indMatchDecoratorXY.hpp"
#include "openMVG/matching/matching_filters.hpp"
#include <openMVG/config.hpp>

#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"
#include "third_party/progress/progress.hpp"

namespace openMVG {
namespace matching_image_collection {

using namespace openMVG::matching;
using namespace openMVG::features;

ImageCollectionMatcher_CascadeHashing
::ImageCollectionMatcher_CascadeHashing
(
  float distRatio
):IImageCollectionMatcher(), f_dist_ratio_(distRatio)
{
}

namespace impl
{
template <typename ScalarT>
void Match
(
  const sfm::SfM_Data & sfm_data,
  const features::RegionsPerView& regionsPerView,
  const Pair_Set & pairs,
  EImageDescriberType descType,
  float fDistRatio,
  PairwiseMatches & map_PutativesMatches // the pairwise photometric corresponding points
)
{
  C_Progress_display my_progress_bar( pairs.size() );

  // Collect used view indexes
  std::set<IndexT> used_index;
  // Sort pairs according the first index to minimize later memory swapping
  typedef std::map<IndexT, std::vector<IndexT> > Map_vectorT;
  Map_vectorT map_Pairs;
  for (Pair_Set::const_iterator iter = pairs.begin(); iter != pairs.end(); ++iter)
  {
    map_Pairs[iter->first].push_back(iter->second);
    used_index.insert(iter->first);
    used_index.insert(iter->second);
  }

  typedef Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> BaseMat;

  // Init the cascade hasher
  CascadeHasher cascade_hasher;
  if (!used_index.empty())
  {
    const IndexT I = *used_index.begin();
    const features::Regions &regionsI = regionsPerView.getRegions(I, descType);
    const size_t dimension = regionsI.DescriptorLength();
    cascade_hasher.Init(dimension);
  }

  std::map<IndexT, HashedDescriptions> hashed_base_;

  // Compute the zero mean descriptor that will be used for hashing (one for all the image regions)
  Eigen::VectorXf zero_mean_descriptor;
  {
    Eigen::MatrixXf matForZeroMean;
    for (int i =0; i < used_index.size(); ++i)
    {
      std::set<IndexT>::const_iterator iter = used_index.begin();
      std::advance(iter, i);
      const IndexT I = *iter;
      const features::Regions &regionsI = regionsPerView.getRegions(I, descType);
      const ScalarT * tabI =
        reinterpret_cast<const ScalarT*>(regionsI.DescriptorRawData());
      const size_t dimension = regionsI.DescriptorLength();
      if (i==0)
      {
        matForZeroMean.resize(used_index.size(), dimension);
        matForZeroMean.fill(0.0f);
      }
      if (regionsI.RegionCount() > 0)
      {
        Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.RegionCount(), dimension);
        matForZeroMean.row(i) = CascadeHasher::GetZeroMeanDescriptor(mat_I);
      }
    }
    zero_mean_descriptor = CascadeHasher::GetZeroMeanDescriptor(matForZeroMean);
  }

  // Index the input regions
  #pragma omp parallel for schedule(dynamic)
  for (int i =0; i < used_index.size(); ++i)
  {
    std::set<IndexT>::const_iterator iter = used_index.begin();
    std::advance(iter, i);
    const IndexT I = *iter;
    const features::Regions &regionsI = regionsPerView.getRegions(I, descType);
    const ScalarT * tabI =
      reinterpret_cast<const ScalarT*>(regionsI.DescriptorRawData());
    const size_t dimension = regionsI.DescriptorLength();

    Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.RegionCount(), dimension);
    HashedDescriptions hashed_description = cascade_hasher.CreateHashedDescriptions(mat_I,
      zero_mean_descriptor);
    #pragma omp critical
    {
      hashed_base_[I] = std::move(hashed_description);
    }
  }

  // Perform matching between all the pairs
  for (Map_vectorT::const_iterator iter = map_Pairs.begin();
    iter != map_Pairs.end(); ++iter)
  {
    const IndexT I = iter->first;
    const std::vector<IndexT> & indexToCompare = iter->second;

    const features::Regions &regionsI = regionsPerView.getRegions(I, descType);
    if (regionsI.RegionCount() == 0)
    {
      my_progress_bar += indexToCompare.size();
      continue;
    }

    const std::vector<features::PointFeature> pointFeaturesI = regionsI.GetRegionsPositions();
    const ScalarT * tabI =
      reinterpret_cast<const ScalarT*>(regionsI.DescriptorRawData());
    const size_t dimension = regionsI.DescriptorLength();
    Eigen::Map<BaseMat> mat_I( (ScalarT*)tabI, regionsI.RegionCount(), dimension);
    #pragma omp parallel for schedule(dynamic)
    for (int j = 0; j < (int)indexToCompare.size(); ++j)
    {
      size_t J = indexToCompare[j];
      const features::Regions &regionsJ = regionsPerView.getRegions(I, descType);

      if (!regionsPerView.viewExist(J)
          || regionsI.Type_id() != regionsJ.Type_id())
      {
        #pragma omp critical
        ++my_progress_bar;
        continue;
      }

      // Matrix representation of the query input data;
      const ScalarT * tabJ = reinterpret_cast<const ScalarT*>(regionsJ.DescriptorRawData());
      Eigen::Map<BaseMat> mat_J( (ScalarT*)tabJ, regionsJ.RegionCount(), dimension);

      IndMatches pvec_indices;
      typedef typename Accumulator<ScalarT>::Type ResultType;
      std::vector<ResultType> pvec_distances;
      pvec_distances.reserve(regionsJ.RegionCount() * 2);
      pvec_indices.reserve(regionsJ.RegionCount() * 2);

      // Match the query descriptors to the database
      cascade_hasher.Match_HashedDescriptions<BaseMat, ResultType>(
        hashed_base_[J], mat_J,
        hashed_base_[I], mat_I,
        &pvec_indices, &pvec_distances);

      std::vector<int> vec_nn_ratio_idx;
      // Filter the matches using a distance ratio test:
      //   The probability that a match is correct is determined by taking
      //   the ratio of distance from the closest neighbor to the distance
      //   of the second closest.
      matching::NNdistanceRatio(
        pvec_distances.begin(), // distance start
        pvec_distances.end(),   // distance end
        2, // Number of neighbor in iterator sequence (minimum required 2)
        vec_nn_ratio_idx, // output (indices that respect the distance Ratio)
        Square(fDistRatio));

      matching::IndMatches vec_putative_matches;
      vec_putative_matches.reserve(vec_nn_ratio_idx.size());
      for (size_t k=0; k < vec_nn_ratio_idx.size(); ++k)
      {
        const size_t index = vec_nn_ratio_idx[k];
        vec_putative_matches.emplace_back(pvec_indices[index*2]._j, pvec_indices[index*2]._i);
      }

      // Remove duplicates
      matching::IndMatch::getDeduplicated(vec_putative_matches);

      // Remove matches that have the same (X,Y) coordinates
      const std::vector<features::PointFeature> pointFeaturesJ = regionsJ.GetRegionsPositions();
      matching::IndMatchDecorator<float> matchDeduplicator(vec_putative_matches,
        pointFeaturesI, pointFeaturesJ);
      matchDeduplicator.getDeduplicated(vec_putative_matches);

      #pragma omp critical
      {
        ++my_progress_bar;
        if (!vec_putative_matches.empty())
        {
          assert(map_PutativesMatches.count(std::make_pair(I,J)) == 0);
          map_PutativesMatches[std::make_pair(I,J)].emplace(descType, std::move(vec_putative_matches));
        }
      }
    }
  }
}
} // namespace impl

void ImageCollectionMatcher_CascadeHashing::Match
(
  const sfm::SfM_Data & sfm_data,
  const features::RegionsPerView& regionsPerView,
  const Pair_Set & pairs,
  features::EImageDescriberType descType,
  PairwiseMatches & map_PutativesMatches // the pairwise photometric corresponding points
) const
{
#if OPENMVG_IS_DEFINED(OPENMVG_USE_OPENMP)
  OPENMVG_LOG_DEBUG("Using the OPENMP thread interface");
#endif

  if (regionsPerView.isEmpty())
    return;

  const features::Regions& regions = regionsPerView.getFirstViewRegions(descType);

  if (regions.IsBinary())
    return;

  if(regions.Type_id() == typeid(unsigned char).name())
  {
    impl::Match<unsigned char>(
      sfm_data,
      regionsPerView,
      pairs,
      descType,
      f_dist_ratio_,
      map_PutativesMatches);
  }
  else
  if(regions.Type_id() == typeid(float).name())
  {
    impl::Match<float>(
      sfm_data,
      regionsPerView,
      pairs,
      descType,
      f_dist_ratio_,
      map_PutativesMatches);
  }
  else
  {
    OPENMVG_LOG_WARNING("Matcher not implemented for this region type");
  }
}

} // namespace openMVG
} // namespace matching_image_collection
<|endoftext|>


<commit_before>/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

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

#include "QmitkAutocropLabelSetImageAction.h"

#include <mitkImagePixelReadAccessor.h>
#include <mitkImageTimeSelector.h>
#include <mitkLabelSetImage.h>
#include <mitkRenderingManager.h>

namespace
{
  // Iterate over all layers, time steps, and dimensions of a LabelSetImage to
  // determine the overall minimum and maximum indices of labeled pixels.
  //
  // Returns false if the input image is empty, minIndex and maxIndex contain
  // valid indices otherwise.
  //
  // Throws an mitk::Exception if read access was denied.
  //
  bool DetermineMinimumAndMaximumIndicesOfNonBackgroundPixels(mitk::LabelSetImage::Pointer labelSetImage, itk::Index<3>& minIndex, itk::Index<3>& maxIndex)
  {
    // We need a time selector to handle 3d+t images. It is not used for 3d images, though.
    auto timeSelector = mitk::ImageTimeSelector::New();
    timeSelector->SetInput(labelSetImage);

    const auto background = labelSetImage->GetExteriorLabel()->GetValue();
    const auto numLayers = labelSetImage->GetNumberOfLayers();
    const auto numTimeSteps = labelSetImage->GetTimeSteps();

    const itk::Index<3> dim = {
      labelSetImage->GetDimension(0),
      labelSetImage->GetDimension(1),
      labelSetImage->GetDimension(2)
    };

    maxIndex = { 0, 0, 0 };
    minIndex = dim;
    itk::Index<3> index;

    bool labelSetImageIsEmpty = true;

    for (std::remove_const_t<decltype(numLayers)> layer = 0; layer < numLayers; ++layer)
    {
      labelSetImage->SetActiveLayer(layer);

      for (std::remove_const_t<decltype(numTimeSteps)> timeStep = 0; timeStep < numTimeSteps; ++timeStep)
      {
        const mitk::Image* image = nullptr;

        if (numTimeSteps > 1)
        {
          timeSelector->SetTimeNr(timeStep);
          timeSelector->Update();
          image = timeSelector->GetOutput();
        }
        else
        {
          image = labelSetImage;
        }

        mitk::ImagePixelReadAccessor<mitk::LabelSetImage::PixelType, 3> pixelReader(image);
        bool imageIsEmpty = true;

        for (index[2] = 0; index[2] < dim[2]; ++index[2])
        {
          for (index[1] = 0; index[1] < dim[1]; ++index[1])
          {
            for (index[0] = 0; index[0] < dim[0]; ++index[0])
            {
              if (background != pixelReader.GetPixelByIndex(index))
              {
                imageIsEmpty = false;
                minIndex = {
                  std::min(minIndex[0], index[0]),
                  std::min(minIndex[1], index[1]),
                  std::min(minIndex[2], index[2])
                };
                break;
              }
            }
          }
        }

        if (imageIsEmpty)
          continue;

        maxIndex = {
          std::max(maxIndex[0], minIndex[0]),
          std::max(maxIndex[1], minIndex[1]),
          std::max(maxIndex[2], minIndex[2])
        };

        for (index[2] = dim[2] - 1; index[2] >= 0; --index[2])
        {
          for (index[1] = dim[1] - 1; index[1] >= 0; --index[1])
          {
            for (index[0] = dim[0] - 1; index[0] >= 0; --index[0])
            {
              if (background != pixelReader.GetPixelByIndex(index))
              {
                maxIndex = {
                  std::max(maxIndex[0], index[0]),
                  std::max(maxIndex[1], index[1]),
                  std::max(maxIndex[2], index[2])
                };
                break;
              }
            }
          }
        }

        if (!imageIsEmpty)
          labelSetImageIsEmpty = false;
      }
    }

    return !labelSetImageIsEmpty;
  }

  // Crop a LabelSetImage. Labels in the cropped LabelSetImage will still have
  // their original properties like names and colors.
  //
  // Returns a cropped LabelSetImage.
  //
  // Throws an mitk::Exception if read access was denied.
  //
  mitk::LabelSetImage::Pointer Crop(mitk::LabelSetImage::Pointer labelSetImage, const itk::Index<3>& minIndex, const itk::Index<3>& maxIndex)
  {
    // We need a time selector to handle 3d+t images. It is not used for 3d images, though.
    auto timeSelector = mitk::ImageTimeSelector::New();
    timeSelector->SetInput(labelSetImage);

    const auto numLayers = labelSetImage->GetNumberOfLayers();
    const auto numTimeSteps = labelSetImage->GetTimeSteps();

    const itk::Index<3> croppedDim = {
      1 + maxIndex[0] - minIndex[0],
      1 + maxIndex[1] - minIndex[1],
      1 + maxIndex[2] - minIndex[2]
    };

    const auto numPixels = croppedDim[0] * croppedDim[1] * croppedDim[2];

    mitk::BaseGeometry::BoundsArrayType croppedBounds;
    croppedBounds[0] = 0;
    croppedBounds[1] = croppedDim[0];
    croppedBounds[2] = 0;
    croppedBounds[3] = croppedDim[1];
    croppedBounds[4] = 0;
    croppedBounds[5] = croppedDim[2];

    // Clone and adapt the original TimeGeometry to the cropped region

    auto croppedTimeGeometry = labelSetImage->GetTimeGeometry()->Clone();

    for (std::remove_const_t<decltype(numTimeSteps)> timeStep = 0; timeStep < numTimeSteps; ++timeStep)
    {
      auto geometry = croppedTimeGeometry->GetGeometryForTimeStep(timeStep);

      mitk::Point3D croppedOrigin;
      geometry->IndexToWorld(minIndex, croppedOrigin);
      geometry->SetOrigin(croppedOrigin);

      geometry->SetBounds(croppedBounds);
    }

    auto croppedLabelSetImage = mitk::LabelSetImage::New();
    croppedLabelSetImage->Initialize(mitk::MakeScalarPixelType<mitk::LabelSetImage::PixelType>(), *croppedTimeGeometry);

    // Create cropped image volumes for all time steps in all layers

    for (std::remove_const_t<decltype(numLayers)> layer = 0; layer < numLayers; ++layer)
    {
      labelSetImage->SetActiveLayer(layer);
      croppedLabelSetImage->AddLayer();

      for (std::remove_const_t<decltype(numTimeSteps)> timeStep = 0; timeStep < numTimeSteps; ++timeStep)
      {
        const mitk::Image* image = nullptr;

        if (numTimeSteps > 1)
        {
          timeSelector->SetTimeNr(timeStep);
          timeSelector->Update();
          image = timeSelector->GetOutput();
        }
        else
        {
          image = labelSetImage;
        }

        mitk::ImagePixelReadAccessor<mitk::LabelSetImage::PixelType, 3> pixelReader(image);
        auto* croppedVolume = new mitk::LabelSetImage::PixelType[numPixels];
        itk::Index<3> croppedIndex;
        itk::Index<3> index;

        for (croppedIndex[2] = 0; croppedIndex[2] < croppedDim[2]; ++croppedIndex[2])
        {
          for (croppedIndex[1] = 0; croppedIndex[1] < croppedDim[1]; ++croppedIndex[1])
          {
            for (croppedIndex[0] = 0; croppedIndex[0] < croppedDim[0]; ++croppedIndex[0])
            {
              index[0] = croppedIndex[0] + minIndex[0];
              index[1] = croppedIndex[1] + minIndex[1];
              index[2] = croppedIndex[2] + minIndex[2];
              const auto& pixel = pixelReader.GetPixelByIndex(index);

              croppedVolume[croppedIndex[2] * croppedDim[1] * croppedDim[0] + croppedIndex[1] * croppedDim[0] + croppedIndex[0]] = pixel;
            }
          }
        }

        croppedLabelSetImage->SetVolume(croppedVolume, timeStep);
        croppedLabelSetImage->AddLabelSetToLayer(layer, labelSetImage->GetLabelSet(layer));
      }
    }

    return croppedLabelSetImage;
  }
}

QmitkAutocropLabelSetImageAction::QmitkAutocropLabelSetImageAction()
{
}

QmitkAutocropLabelSetImageAction::~QmitkAutocropLabelSetImageAction()
{
}

void QmitkAutocropLabelSetImageAction::Run(const QList<mitk::DataNode::Pointer>& selectedNodes)
{
  for (const auto& dataNode : selectedNodes)
  {
    mitk::LabelSetImage::Pointer labelSetImage = dynamic_cast<mitk::LabelSetImage*>(dataNode->GetData());

    if (labelSetImage.IsNull())
      continue;

    // Backup currently active layer as we need to restore it later
    auto activeLayer = labelSetImage->GetActiveLayer();

    mitk::LabelSetImage::Pointer croppedLabelSetImage;
    itk::Index<3> minIndex;
    itk::Index<3> maxIndex;

    try
    {
      if (!DetermineMinimumAndMaximumIndicesOfNonBackgroundPixels(labelSetImage, minIndex, maxIndex))
      {
        MITK_WARN << "Autocrop was skipped: Image \"" << dataNode->GetName() << "\" is empty.";
        labelSetImage->SetActiveLayer(activeLayer); // Restore the originally active layer
        return;
      }

      croppedLabelSetImage = Crop(labelSetImage, minIndex, maxIndex);
    }
    catch (const mitk::Exception&)
    {
      MITK_ERROR << "Autocrop was aborted: Image read access to \"" << dataNode->GetName() << "\" was denied.";
      labelSetImage->SetActiveLayer(activeLayer); // Restore the originally active layer
      return;
    }

    // Restore the originally active layer in the cropped LabelSetImage
    croppedLabelSetImage->SetActiveLayer(activeLayer);

    // Override the original LabelSetImage with the cropped LabelSetImage
    dataNode->SetData(croppedLabelSetImage);

    // If we cropped a single LabelSetImage, reinit the views to give a visible feedback to the user
    if (1 == selectedNodes.size())
      mitk::RenderingManager::GetInstance()->InitializeViews(croppedLabelSetImage->GetTimeGeometry(), mitk::RenderingManager::REQUEST_UPDATE_ALL, true);
  }
}

void QmitkAutocropLabelSetImageAction::SetSmoothed(bool)
{
}

void QmitkAutocropLabelSetImageAction::SetDecimated(bool)
{
}

void QmitkAutocropLabelSetImageAction::SetDataStorage(mitk::DataStorage*)
{
}

void QmitkAutocropLabelSetImageAction::SetFunctionality(berry::QtViewPart*)
{
}
<commit_msg>Fix newly introduced memory leak<commit_after>/*============================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.

Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.

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

#include "QmitkAutocropLabelSetImageAction.h"

#include <mitkImagePixelReadAccessor.h>
#include <mitkImageTimeSelector.h>
#include <mitkLabelSetImage.h>
#include <mitkRenderingManager.h>

namespace
{
  // Iterate over all layers, time steps, and dimensions of a LabelSetImage to
  // determine the overall minimum and maximum indices of labeled pixels.
  //
  // Returns false if the input image is empty, minIndex and maxIndex contain
  // valid indices otherwise.
  //
  // Throws an mitk::Exception if read access was denied.
  //
  bool DetermineMinimumAndMaximumIndicesOfNonBackgroundPixels(mitk::LabelSetImage::Pointer labelSetImage, itk::Index<3>& minIndex, itk::Index<3>& maxIndex)
  {
    // We need a time selector to handle 3d+t images. It is not used for 3d images, though.
    auto timeSelector = mitk::ImageTimeSelector::New();
    timeSelector->SetInput(labelSetImage);

    const auto background = labelSetImage->GetExteriorLabel()->GetValue();
    const auto numLayers = labelSetImage->GetNumberOfLayers();
    const auto numTimeSteps = labelSetImage->GetTimeSteps();

    const itk::Index<3> dim = {
      labelSetImage->GetDimension(0),
      labelSetImage->GetDimension(1),
      labelSetImage->GetDimension(2)
    };

    maxIndex = { 0, 0, 0 };
    minIndex = dim;
    itk::Index<3> index;

    bool labelSetImageIsEmpty = true;

    for (std::remove_const_t<decltype(numLayers)> layer = 0; layer < numLayers; ++layer)
    {
      labelSetImage->SetActiveLayer(layer);

      for (std::remove_const_t<decltype(numTimeSteps)> timeStep = 0; timeStep < numTimeSteps; ++timeStep)
      {
        const mitk::Image* image = nullptr;

        if (numTimeSteps > 1)
        {
          timeSelector->SetTimeNr(timeStep);
          timeSelector->Update();
          image = timeSelector->GetOutput();
        }
        else
        {
          image = labelSetImage;
        }

        mitk::ImagePixelReadAccessor<mitk::LabelSetImage::PixelType, 3> pixelReader(image);
        bool imageIsEmpty = true;

        for (index[2] = 0; index[2] < dim[2]; ++index[2])
        {
          for (index[1] = 0; index[1] < dim[1]; ++index[1])
          {
            for (index[0] = 0; index[0] < dim[0]; ++index[0])
            {
              if (background != pixelReader.GetPixelByIndex(index))
              {
                imageIsEmpty = false;
                minIndex = {
                  std::min(minIndex[0], index[0]),
                  std::min(minIndex[1], index[1]),
                  std::min(minIndex[2], index[2])
                };
                break;
              }
            }
          }
        }

        if (imageIsEmpty)
          continue;

        maxIndex = {
          std::max(maxIndex[0], minIndex[0]),
          std::max(maxIndex[1], minIndex[1]),
          std::max(maxIndex[2], minIndex[2])
        };

        for (index[2] = dim[2] - 1; index[2] >= 0; --index[2])
        {
          for (index[1] = dim[1] - 1; index[1] >= 0; --index[1])
          {
            for (index[0] = dim[0] - 1; index[0] >= 0; --index[0])
            {
              if (background != pixelReader.GetPixelByIndex(index))
              {
                maxIndex = {
                  std::max(maxIndex[0], index[0]),
                  std::max(maxIndex[1], index[1]),
                  std::max(maxIndex[2], index[2])
                };
                break;
              }
            }
          }
        }

        if (!imageIsEmpty)
          labelSetImageIsEmpty = false;
      }
    }

    return !labelSetImageIsEmpty;
  }

  // Crop a LabelSetImage. Labels in the cropped LabelSetImage will still have
  // their original properties like names and colors.
  //
  // Returns a cropped LabelSetImage.
  //
  // Throws an mitk::Exception if read access was denied.
  //
  mitk::LabelSetImage::Pointer Crop(mitk::LabelSetImage::Pointer labelSetImage, const itk::Index<3>& minIndex, const itk::Index<3>& maxIndex)
  {
    // We need a time selector to handle 3d+t images. It is not used for 3d images, though.
    auto timeSelector = mitk::ImageTimeSelector::New();
    timeSelector->SetInput(labelSetImage);

    const auto numLayers = labelSetImage->GetNumberOfLayers();
    const auto numTimeSteps = labelSetImage->GetTimeSteps();

    const itk::Index<3> croppedDim = {
      1 + maxIndex[0] - minIndex[0],
      1 + maxIndex[1] - minIndex[1],
      1 + maxIndex[2] - minIndex[2]
    };

    const auto numPixels = croppedDim[0] * croppedDim[1] * croppedDim[2];

    mitk::BaseGeometry::BoundsArrayType croppedBounds;
    croppedBounds[0] = 0;
    croppedBounds[1] = croppedDim[0];
    croppedBounds[2] = 0;
    croppedBounds[3] = croppedDim[1];
    croppedBounds[4] = 0;
    croppedBounds[5] = croppedDim[2];

    // Clone and adapt the original TimeGeometry to the cropped region

    auto croppedTimeGeometry = labelSetImage->GetTimeGeometry()->Clone();

    for (std::remove_const_t<decltype(numTimeSteps)> timeStep = 0; timeStep < numTimeSteps; ++timeStep)
    {
      auto geometry = croppedTimeGeometry->GetGeometryForTimeStep(timeStep);

      mitk::Point3D croppedOrigin;
      geometry->IndexToWorld(minIndex, croppedOrigin);
      geometry->SetOrigin(croppedOrigin);

      geometry->SetBounds(croppedBounds);
    }

    auto croppedLabelSetImage = mitk::LabelSetImage::New();
    croppedLabelSetImage->Initialize(mitk::MakeScalarPixelType<mitk::LabelSetImage::PixelType>(), *croppedTimeGeometry);

    // Create cropped image volumes for all time steps in all layers

    for (std::remove_const_t<decltype(numLayers)> layer = 0; layer < numLayers; ++layer)
    {
      labelSetImage->SetActiveLayer(layer);
      croppedLabelSetImage->AddLayer();

      for (std::remove_const_t<decltype(numTimeSteps)> timeStep = 0; timeStep < numTimeSteps; ++timeStep)
      {
        const mitk::Image* image = nullptr;

        if (numTimeSteps > 1)
        {
          timeSelector->SetTimeNr(timeStep);
          timeSelector->Update();
          image = timeSelector->GetOutput();
        }
        else
        {
          image = labelSetImage;
        }

        mitk::ImagePixelReadAccessor<mitk::LabelSetImage::PixelType, 3> pixelReader(image);
        auto* croppedVolume = new mitk::LabelSetImage::PixelType[numPixels];
        itk::Index<3> croppedIndex;
        itk::Index<3> index;

        for (croppedIndex[2] = 0; croppedIndex[2] < croppedDim[2]; ++croppedIndex[2])
        {
          for (croppedIndex[1] = 0; croppedIndex[1] < croppedDim[1]; ++croppedIndex[1])
          {
            for (croppedIndex[0] = 0; croppedIndex[0] < croppedDim[0]; ++croppedIndex[0])
            {
              index[0] = croppedIndex[0] + minIndex[0];
              index[1] = croppedIndex[1] + minIndex[1];
              index[2] = croppedIndex[2] + minIndex[2];
              const auto& pixel = pixelReader.GetPixelByIndex(index);

              croppedVolume[croppedIndex[2] * croppedDim[1] * croppedDim[0] + croppedIndex[1] * croppedDim[0] + croppedIndex[0]] = pixel;
            }
          }
        }

        croppedLabelSetImage->SetImportVolume(croppedVolume, timeStep, 0, mitk::Image::ReferenceMemory);
        croppedLabelSetImage->AddLabelSetToLayer(layer, labelSetImage->GetLabelSet(layer));
      }
    }

    return croppedLabelSetImage;
  }
}

QmitkAutocropLabelSetImageAction::QmitkAutocropLabelSetImageAction()
{
}

QmitkAutocropLabelSetImageAction::~QmitkAutocropLabelSetImageAction()
{
}

void QmitkAutocropLabelSetImageAction::Run(const QList<mitk::DataNode::Pointer>& selectedNodes)
{
  for (const auto& dataNode : selectedNodes)
  {
    mitk::LabelSetImage::Pointer labelSetImage = dynamic_cast<mitk::LabelSetImage*>(dataNode->GetData());

    if (labelSetImage.IsNull())
      continue;

    // Backup currently active layer as we need to restore it later
    auto activeLayer = labelSetImage->GetActiveLayer();

    mitk::LabelSetImage::Pointer croppedLabelSetImage;
    itk::Index<3> minIndex;
    itk::Index<3> maxIndex;

    try
    {
      if (!DetermineMinimumAndMaximumIndicesOfNonBackgroundPixels(labelSetImage, minIndex, maxIndex))
      {
        MITK_WARN << "Autocrop was skipped: Image \"" << dataNode->GetName() << "\" is empty.";
        labelSetImage->SetActiveLayer(activeLayer); // Restore the originally active layer
        return;
      }

      croppedLabelSetImage = Crop(labelSetImage, minIndex, maxIndex);
    }
    catch (const mitk::Exception&)
    {
      MITK_ERROR << "Autocrop was aborted: Image read access to \"" << dataNode->GetName() << "\" was denied.";
      labelSetImage->SetActiveLayer(activeLayer); // Restore the originally active layer
      return;
    }

    // Restore the originally active layer in the cropped LabelSetImage
    croppedLabelSetImage->SetActiveLayer(activeLayer);

    // Override the original LabelSetImage with the cropped LabelSetImage
    dataNode->SetData(croppedLabelSetImage);

    // If we cropped a single LabelSetImage, reinit the views to give a visible feedback to the user
    if (1 == selectedNodes.size())
      mitk::RenderingManager::GetInstance()->InitializeViews(croppedLabelSetImage->GetTimeGeometry(), mitk::RenderingManager::REQUEST_UPDATE_ALL, true);
  }
}

void QmitkAutocropLabelSetImageAction::SetSmoothed(bool)
{
}

void QmitkAutocropLabelSetImageAction::SetDecimated(bool)
{
}

void QmitkAutocropLabelSetImageAction::SetDataStorage(mitk::DataStorage*)
{
}

void QmitkAutocropLabelSetImageAction::SetFunctionality(berry::QtViewPart*)
{
}
<|endoftext|>