py::EigenDMap<Eigen::Matrix2d> corners() { return py::EigenDMap<Eigen::Matrix2d>(mat.data(),
py::EigenDStride(mat.outerStride() * (mat.outerSize()-1), mat.innerStride() * (mat.innerSize()-1))); }
// OUT' = bcsr * B' (for matrices)
void A_mul_Bt(Eigen::MatrixXd & out, CSR & csr, Eigen::MatrixXd & B) {
if (csr.nrow != out.cols()) {throw std::runtime_error("csr.nrow must equal out.cols()");}
if (csr.ncol != B.cols()) {throw std::runtime_error("csr.ncol must equal b.cols()");}
if (out.rows() != B.rows()) {throw std::runtime_error("out.rows() must equal B.rows()");}
csr_A_mul_Bn( out.data(), & csr, B.data(), B.rows() );
}
bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
moveit_msgs::MotionPlanDetailedResponse& res) const
{
if (!planning_scene)
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized.");
res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
return false;
}
if (req.start_state.joint_state.position.empty())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state is empty");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
return false;
}
if (not planning_scene->getRobotModel()->satisfiesPositionBounds(req.start_state.joint_state.position.data()))
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state violates joint limits");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
return false;
}
ros::WallTime start_time = ros::WallTime::now();
ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);
jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name,
trajectory.getTrajectoryPoint(0));
if (req.goal_constraints.empty())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "No goal constraints specified!");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
return false;
}
if (req.goal_constraints[0].joint_constraints.empty())
{
ROS_ERROR_STREAM("Only joint-space goals are supported");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
return false;
}
int goal_index = trajectory.getNumPoints() - 1;
trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
sensor_msgs::JointState js;
for (const moveit_msgs::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints)
{
js.name.push_back(joint_constraint.joint_name);
js.position.push_back(joint_constraint.position);
ROS_INFO_STREAM_NAMED("chomp_planner", "Setting joint " << joint_constraint.joint_name << " to position "
<< joint_constraint.position);
}
jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index));
const moveit::core::JointModelGroup* model_group =
planning_scene->getRobotModel()->getJointModelGroup(req.group_name);
// fix the goal to move the shortest angular distance for wrap-around joints:
for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++)
{
const moveit::core::JointModel* model = model_group->getActiveJointModels()[i];
const moveit::core::RevoluteJointModel* revolute_joint =
dynamic_cast<const moveit::core::RevoluteJointModel*>(model);
if (revolute_joint != nullptr)
{
if (revolute_joint->isContinuous())
{
double start = (trajectory)(0, i);
double end = (trajectory)(goal_index, i);
ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
(trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
}
}
}
const std::vector<std::string>& active_joint_names = model_group->getActiveJointModelNames();
const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index);
moveit::core::RobotState goal_robot_state = planning_scene->getCurrentState();
goal_robot_state.setVariablePositions(
active_joint_names, std::vector<double>(goal_state.data(), goal_state.data() + active_joint_names.size()));
if (not goal_robot_state.satisfiesBounds())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
return false;
}
// fill in an initial trajectory based on user choice from the chomp_config.yaml file
if (params.trajectory_initialization_method_.compare("quintic-spline") == 0)
trajectory.fillInMinJerk();
else if (params.trajectory_initialization_method_.compare("linear") == 0)
trajectory.fillInLinearInterpolation();
else if (params.trajectory_initialization_method_.compare("cubic") == 0)
trajectory.fillInCubicInterpolation();
else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
{
if (!(trajectory.fillInFromTrajectory(res)))
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Input trajectory has less than 2 points, "
"trajectory must contain at least start and goal state");
return false;
}
}
else
ROS_ERROR_STREAM_NAMED("chomp_planner", "invalid interpolation method specified in the chomp_planner file");
ROS_INFO_NAMED("chomp_planner", "CHOMP trajectory initialized using method: %s ",
(params.trajectory_initialization_method_).c_str());
// optimize!
moveit::core::RobotState start_state(planning_scene->getCurrentState());
moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
start_state.update();
ros::WallTime create_time = ros::WallTime::now();
int replan_count = 0;
bool replan_flag = false;
double org_learning_rate = 0.04, org_ridge_factor = 0.0, org_planning_time_limit = 10;
int org_max_iterations = 200;
// storing the initial chomp parameters values
org_learning_rate = params.learning_rate_;
org_ridge_factor = params.ridge_factor_;
org_planning_time_limit = params.planning_time_limit_;
org_max_iterations = params.max_iterations_;
std::unique_ptr<ChompOptimizer> optimizer;
// create a non_const_params variable which stores the non constant version of the const params variable
ChompParameters params_nonconst = params;
// while loop for replanning (recovery behaviour) if collision free optimized solution not found
while (true)
{
if (replan_flag)
{
// increase learning rate in hope to find a successful path; increase ridge factor to avoid obstacles; add 5
// additional secs in hope to find a solution; increase maximum iterations
params_nonconst.setRecoveryParams(params_nonconst.learning_rate_ + 0.02, params_nonconst.ridge_factor_ + 0.002,
params_nonconst.planning_time_limit_ + 5, params_nonconst.max_iterations_ + 50);
}
// initialize a ChompOptimizer object to load up the optimizer with default parameters or with updated parameters in
// case of a recovery behaviour
optimizer.reset(new ChompOptimizer(&trajectory, planning_scene, req.group_name, ¶ms_nonconst, start_state));
if (!optimizer->isInitialized())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Could not initialize optimizer");
res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
return false;
}
ROS_DEBUG_NAMED("chomp_planner", "Optimization took %f sec to create",
(ros::WallTime::now() - create_time).toSec());
bool optimization_result = optimizer->optimize();
// replan with updated parameters if no solution is found
if (params_nonconst.enable_failure_recovery_)
{
ROS_INFO_NAMED("chomp_planner", "Planned with Chomp Parameters (learning_rate, ridge_factor, "
"planning_time_limit, max_iterations), attempt: # %d ",
(replan_count + 1));
ROS_INFO_NAMED("chomp_planner", "Learning rate: %f ridge factor: %f planning time limit: %f max_iterations %d ",
params_nonconst.learning_rate_, params_nonconst.ridge_factor_,
params_nonconst.planning_time_limit_, params_nonconst.max_iterations_);
if (!optimization_result && replan_count < params_nonconst.max_recovery_attempts_)
{
replan_count++;
replan_flag = true;
}
else
{
break;
}
}
else
break;
} // end of while loop
// resetting the CHOMP Parameters to the original values after a successful plan
params_nonconst.setRecoveryParams(org_learning_rate, org_ridge_factor, org_planning_time_limit, org_max_iterations);
ROS_DEBUG_NAMED("chomp_planner", "Optimization actually took %f sec to run",
(ros::WallTime::now() - create_time).toSec());
create_time = ros::WallTime::now();
// assume that the trajectory is now optimized, fill in the output structure:
ROS_DEBUG_NAMED("chomp_planner", "Output trajectory has %d joints", trajectory.getNumJoints());
res.trajectory.resize(1);
res.trajectory[0].joint_trajectory.joint_names = active_joint_names;
res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; // @TODO this is probably a hack
// fill in the entire trajectory
res.trajectory[0].joint_trajectory.points.resize(trajectory.getNumPoints());
for (int i = 0; i < trajectory.getNumPoints(); i++)
{
res.trajectory[0].joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
for (size_t j = 0; j < res.trajectory[0].joint_trajectory.points[i].positions.size(); j++)
{
res.trajectory[0].joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
}
// Setting invalid timestamps.
// Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
}
ROS_DEBUG_NAMED("chomp_planner", "Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
ROS_DEBUG_NAMED("chomp_planner", "Serviced planning request in %f wall-seconds, trajectory duration is %f",
(ros::WallTime::now() - start_time).toSec(),
res.trajectory[0].joint_trajectory.points[goal_index].time_from_start.toSec());
res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
res.processing_time.push_back((ros::WallTime::now() - start_time).toSec());
// report planning failure if path has collisions
if (not optimizer->isCollisionFree())
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Motion plan is invalid.");
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
return false;
}
// check that final state is within goal tolerances
kinematic_constraints::JointConstraint jc(planning_scene->getRobotModel());
robot_state::RobotState last_state(start_state);
last_state.setVariablePositions(res.trajectory[0].joint_trajectory.joint_names,
res.trajectory[0].joint_trajectory.points.back().positions);
bool constraints_are_ok = true;
for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints[0].joint_constraints)
{
constraints_are_ok = constraints_are_ok and jc.configure(constraint);
constraints_are_ok = constraints_are_ok and jc.decide(last_state).satisfied;
if (not constraints_are_ok)
{
ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal constraints are violated: " << constraint.joint_name);
res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
return false;
}
}
return true;
}
double mp2_gamma_ind_p(
UnitCell& UCell,
SuperCell& SCell,
VMatrixXd& evals,
std::string mointfile,
bool read_in_from_file,
moIntegralFactory& moint_class,
int argc, char *argv[]
){
//std::cout << "INITIALIZING MPI...." << std::endl;
int numtasks, taskid, source;
//MPI_Init( &argc, &argv );
MPI_Comm_size( MPI_COMM_WORLD, &numtasks );
std::cout << "MP2 : RUNNING " << numtasks << " TASKS WITH MPI." << std::endl;
MPI_Comm_rank( MPI_COMM_WORLD, &taskid );
std::cout << "MP2 : TASK " << taskid << " STARTED!" << std::endl;
MPI_Status status;
int tag1, tag2, tag3, tag4, tag5;
tag1 = 1;
tag2 = 2;
tag3 = 3;
tag4 = 4;
tag5 = 5;
double mp2en = 0.0;
double total_mp2en = 0.0;
int nocc, norb;
std::vector< double > eigenvals;
Eigen::MatrixXd kernel_matr;
Eigen::MatrixXd evecsXd;
std::vector< double > kernel_matr_data;
std::vector< double > evecsXd_data;
if( taskid == 0 ){
nocc = (int)(SCell.nao/2);
norb = SCell.nao;
eigenvals.resize( norb );
for( int i = 0; i < norb; ++i ) eigenvals[ i ] = evals.irrep( 0 )( i, 0 );
kernel_matr = moint_class.get_kernel_matr();
evecsXd = moint_class.get_evecsXd();
/* Send these quantities to all tasks */
for( int dest = 1; dest < numtasks; ++dest ){
cout << "SENDING DATA TO TASK " << taskid << endl;
MPI_Send( &nocc, 1, MPI_INT, dest, tag1, MPI_COMM_WORLD );
MPI_Send( &norb, 1, MPI_INT, dest, tag2, MPI_COMM_WORLD );
MPI_Send( &eigenvals[ 0 ], norb, MPI_DOUBLE, dest, tag3, MPI_COMM_WORLD );
//for( int i = 0; i < norb; ++i ) cout << "TASK " << taskid << " EIGENVALUE " << i << " : " << eigenvals[ i ] << endl;
MPI_Send( kernel_matr.data(), norb*norb, MPI_DOUBLE, dest, tag4, MPI_COMM_WORLD );
MPI_Send( evecsXd.data(), norb*norb, MPI_DOUBLE, dest, tag5, MPI_COMM_WORLD );
cout << "DONE : SENDING DATA TO TASK " << taskid << endl;
}
}
if( taskid > 0 ){
for( int dest = 1; dest < numtasks; ++dest ){
if( taskid == dest ){
cout << "TASK " << dest << " RECIEVING DATA" << endl;
MPI_Recv( &nocc, 1, MPI_INT, 0, tag1, MPI_COMM_WORLD, &status );
MPI_Recv( &norb, 1, MPI_INT, 0, tag2, MPI_COMM_WORLD, &status );
eigenvals.resize( norb );
kernel_matr_data.resize( norb * norb );
evecsXd_data.resize( norb * norb );
MPI_Recv( &eigenvals[ 0 ], norb, MPI_DOUBLE, 0, tag3, MPI_COMM_WORLD, &status );
//for( int i = 0; i < norb; ++i ) cout << "TASK " << taskid << " EIGENVALUE " << i << " : " << eigenvals[ i ] << endl;
MPI_Recv( &kernel_matr_data[ 0 ], norb*norb, MPI_DOUBLE, 0, tag4, MPI_COMM_WORLD, &status );
//for( int i = 0; i < norb*norb; ++i ) cout << "TASK " << taskid << " KERNEL VAL " << i << " : " << kernel_matr_data[ i ] << endl;
MPI_Recv( &evecsXd_data[ 0 ], norb*norb, MPI_DOUBLE, 0, tag5, MPI_COMM_WORLD, &status );
//for( int i = 0; i < norb*norb; ++i ) cout << "TASK " << taskid << " EVEVCS_DATA " << i << " : " << evecsXd_data[ i ] << endl;
kernel_matr.resize( norb, norb );
evecsXd.resize( norb, norb );
int counter = 0;
for( int i = 0; i < norb; ++i )
for( int j = 0; j < norb; ++j, counter++ ){
kernel_matr( j, i ) = kernel_matr_data[ counter ];
evecsXd( j, i ) = evecsXd_data[ counter ];
}
//cout << taskid << " KERNEL MATRIX : " << endl;
//cout << kernel_matr.block( 0, 0, 3, 3 ) << endl;
//cout << taskid << " EVECS MATRIX : " << endl;
//cout << evecsXd.block( 0, 0, 3, 3 ) << endl;
cout << "DONE : TASK " << dest << " RECIEVING DATA" << endl;
}
}
}
MPI_Barrier( MPI_COMM_WORLD );
int mo_index_in_arr;
double denom, moint1, moint2;
size_t moindex1, moindex2;
std::vector< size_t >::iterator it;
bool sort_mo = true;
clock_t t, timer;
size_t opt_size_arr, size_arr;
std::vector< size_t > indx_arr;
std::vector< double > dble_arr;
bool optimize_size_arr;
if( read_in_from_file ){
opt_size_arr = 0;
optimize_size_arr = true;
std::cout << "READING FROM MULTIPLE MOINT FILES" << std::endl;
for( int i = 0; i < SCell.nao; ++i ){
read_gamma_mointb_ind_p( mointfile, i, dble_arr, indx_arr, size_arr, optimize_size_arr );
if( size_arr > opt_size_arr ) opt_size_arr = size_arr;
}
std::cout << "OPTIMAL SIZE OF ARRAYS " << opt_size_arr << std::endl;
optimize_size_arr = false;
dble_arr.resize( opt_size_arr );
indx_arr.resize( opt_size_arr );
}else{
dble_arr.resize( 5000000 );
indx_arr.resize( 5000000 );
}
clock_t total_time;
if( taskid == 0 )
total_time = clock();
std::vector< int > break_up_work;
break_up_work.resize( numtasks + 1 );
break_up_work[ 0 ] = nocc;
break_up_work[ numtasks ] = norb;
int break_up_load = (int)( ( norb - nocc ) / numtasks );
for( int i = 1; i < break_up_work.size() - 1; ++i ) break_up_work[ i ] = break_up_work[ i - 1 ] + break_up_load;
//for( int a = nocc; a < norb; ++a ){
for( int i = 0; i < numtasks; ++i ){
if( taskid == i ){
int low = break_up_work[ i ];
int high = break_up_work[ (i + 1) ];
for( int a = low; a < high; ++a ){
cout << "MP2 COMPLETED ( " << setw( 3 ) << ( a - nocc + 1 ) << " / " << setw( 3 ) << ( norb - nocc ) << " ) " << endl;
if( read_in_from_file ){
read_gamma_mointb_ind_p( mointfile, a, dble_arr, indx_arr, size_arr, optimize_size_arr );
}else{
t = clock();
//moint_class.gamma_moint_to_vector( dble_arr, indx_arr, size_arr,
// a, (a+1), 0, nocc,
// nocc, (a+1), 0, nocc, -10 );
gamma_moint_to_vector( dble_arr, indx_arr, kernel_matr, evecsXd, norb, size_arr,
a, (a+1), 0, nocc,
nocc, (a+1), 0, nocc, -10 );
t = clock() - t;
if( taskid == 7 ){
cout << " MOINT GENERATION FOR TASK " << taskid << endl;
cout << " - time = " << t / (double)CLOCKS_PER_SEC << " seconds." << endl;
cout << " - # non-zero = " << size_arr << endl;
cout << " - 'a' virt val= " << a << endl;
}
}
t = clock();
if( sort_mo ){ sorter( dble_arr, indx_arr, size_arr ); }
t = clock() - t;
double time_sort = t / (double)CLOCKS_PER_SEC;
//if( sort_mo ) cout << " o TIME FOR MOINT SORTING : " << time_sort << endl;
t = clock();
for( int i = 0; i < nocc; ++i ){
for( int j = 0; j < nocc; ++j ){
for( int b = nocc; b <= a; ++b ){
moint1 = 0.0;
moint2 = 0.0;
denom = eigenvals[ i ] + eigenvals[ j ] - eigenvals[ a ] - eigenvals[ b ];
moindex1 = get_ijkl( i, a, j, b );
//
//
// NEEDS TO BE ORDERED FOR BINARY SEARCH
//
//
if( sort_mo ){
mo_index_in_arr = binary_search( indx_arr, moindex1, (size_t)0, size_arr-1 );
if( mo_index_in_arr >= 0 ){
moint1 = dble_arr[ mo_index_in_arr ];
}else{
//if( taskid == 7 ){ it = find( indx_arr.begin(), indx_arr.begin() + size_arr, moindex1 );
//if( it != ( indx_arr.begin() + size_arr ) ){
// cout << "YA GOOFED" << endl;
// exit( EXIT_FAILURE );
//}
//}
}
}else{
it = find( indx_arr.begin(), indx_arr.begin() + size_arr, moindex1 );
if( it != ( indx_arr.begin() + size_arr ) ){
mo_index_in_arr = it - indx_arr.begin();
moint1 = dble_arr[ mo_index_in_arr ];
}
}
moindex2 = get_ijkl( j, a, i, b );
//
//
// NEEDS TO BE ORDERED FOR BINARY SEARCH
//
//
if( sort_mo ){
mo_index_in_arr = binary_search( indx_arr, moindex2, (size_t)0, size_arr-1 );
if( mo_index_in_arr >= 0 ){
moint2 = dble_arr[ mo_index_in_arr ];
}
}else{
it = find( indx_arr.begin(), indx_arr.begin() + size_arr, moindex2 );
if( it != ( indx_arr.begin() + size_arr ) ){
mo_index_in_arr = it - indx_arr.begin();
moint2 = dble_arr[ mo_index_in_arr ];
}
}
if( a == b )
mp2en += ( moint1 * ( 2. * moint1 - moint2 ) ) / denom;
else
mp2en += 2. * ( moint1 * ( 2. * moint1 - moint2 ) ) / denom;
//if( taskid == 7 ) cout << "TASK 7 : " << moint1 << " " << moint2 << " " << denom << " " << eigenvals[ j ] << " " << \
eigenvals[ i ] << " " << eigenvals[ a ] << " " << eigenvals[ b ] << " " << mp2en << endl;
}
}
}
t = clock() - t;
double time_other = t / (double)CLOCKS_PER_SEC;
//cout << " o OTHER MP2 TIME : " << time_other << endl;
//cout << " o TOTAL MP2 TIME : " << ( time_other + time_sort ) << endl;
} // end loop over 'a', and so we have this contribution now to mp2 energy
cout << "TASK " << taskid << " MP2 ENERGY : " << setw( 20 ) << setprecision( 16 ) << mp2en << endl;
MPI_Reduce( &mp2en, &total_mp2en, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD );
} // end if == taskid
} // end loop over tasks
//MPI_Finalize();
MPI_Barrier( MPI_COMM_WORLD );
return total_mp2en;
}
void CloudAnalyzer2D::examineFreeSpaceEvidence() {
freeSpaceEvidence.clear();
Eigen::Vector3f cameraCenter = -1.0 * pointMin;
voxel::DirectVoxel<char> freeSpace(numX, numY, numZ);
for (int k = 0; k < numZ; ++k) {
for (int j = 0; j < numY; ++j) {
for (int i = 0; i < numX; ++i) {
if (!pointInVoxel->at(i, j, k))
continue;
Eigen::Vector3d ray(i, j, k);
ray[0] -= cameraCenter[0] * FLAGS_scale;
ray[1] -= cameraCenter[1] * FLAGS_scale;
ray[2] -= cameraCenter[2] * zScale;
double length = ray.norm();
Eigen::Vector3d unitRay = ray / length;
Eigen::Vector3i voxelHit;
for (int a = 0; a <= ceil(length); ++a) {
voxelHit[0] = floor(cameraCenter[0] * FLAGS_scale + a * unitRay[0]);
voxelHit[1] = floor(cameraCenter[1] * FLAGS_scale + a * unitRay[1]);
voxelHit[2] = floor(cameraCenter[2] * zScale + a * unitRay[2]);
if (voxelHit[0] < 0 || voxelHit[0] >= numX)
continue;
if (voxelHit[1] < 0 || voxelHit[1] >= numY)
continue;
if (voxelHit[2] < 0 || voxelHit[2] >= numZ)
continue;
freeSpace(voxelHit) = 1;
}
}
}
}
for (int r = 0; r < R->size(); ++r) {
Eigen::MatrixXd collapsedCount = Eigen::MatrixXd::Zero(newRows, newCols);
#pragma omp parallel for schedule(static)
for (int i = 0; i < collapsedCount.cols(); ++i) {
for (int j = 0; j < collapsedCount.rows(); ++j) {
for (int k = 0; k < numZ; ++k) {
const Eigen::Vector3d coord(i, j, k);
const Eigen::Vector3i src =
(R->at(r) * (coord - newZZ) + zeroZero)
.unaryExpr([](auto v) { return std::round(v); })
.cast<int>();
if (src[0] < 0 || src[0] >= numX || src[1] < 0 || src[1] >= numY ||
src[2] < 0 || src[2] >= numZ)
continue;
if (freeSpace(src))
++collapsedCount(j, i);
}
}
}
double average, sigma;
const double *vPtr = collapsedCount.data();
std::tie(average, sigma) = place::aveAndStdev(
vPtr, vPtr + collapsedCount.size(), [](double v) { return v; },
[](double v) -> bool { return v; });
cv::Mat heatMap(newRows, newCols, CV_8UC1, cv::Scalar::all(255));
for (int j = 0; j < heatMap.rows; ++j) {
uchar *dst = heatMap.ptr<uchar>(j);
for (int i = 0; i < heatMap.cols; ++i) {
const double count = collapsedCount(j, i);
if (count > 0) {
const int gray = cv::saturate_cast<uchar>(
255.0 * ((count - average) / sigma + 1.0));
dst[i] = 255 - gray;
}
}
}
const double radius = 0.3;
for (int j = -sqrt(radius) * FLAGS_scale; j < sqrt(radius) * FLAGS_scale;
++j) {
uchar *dst = heatMap.ptr<uchar>(j + imageZeroZero[1]);
for (int i = -sqrt(radius * FLAGS_scale * FLAGS_scale - j * j);
i < sqrt(radius * FLAGS_scale * FLAGS_scale - j * j); ++i) {
dst[i + imageZeroZero[0]] = 0;
}
}
if (FLAGS_preview) {
cvNamedWindow("Preview", CV_WINDOW_NORMAL);
cv::imshow("Preview", heatMap);
cv::waitKey(0);
}
freeSpaceEvidence.push_back(heatMap);
}
}