bool mrpt::vision::pnp::ppnp::compute_pose(
Eigen::Matrix3d& R, Eigen::Vector3d& t, int n)
{
double tol = 0.00001;
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(n, n), A(n, n), Y(n, 3),
E(n, 3), E_old(n, 3), U, V,
I3 = Eigen::MatrixXd::Identity(3, 3), PR,
Z = Eigen::MatrixXd::Zero(n, n);
Eigen::VectorXd e(n), II(n), c(3), Zmindiag(n);
e.fill(1);
II.fill(1.0 / ((double)n));
A = I - e * e.transpose() / n;
double err = std::numeric_limits<double>::infinity();
E_old.fill(1000);
int cnt = 0;
while (err > tol)
{
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
P.transpose() * Z * A * S,
Eigen::ComputeThinU | Eigen::ComputeThinV);
U = svd.matrixU();
V = svd.matrixV();
I3(2, 2) = (U * V.transpose()).determinant();
R = U * I3 * V.transpose();
PR = P * R;
c = (S - Z * PR).transpose() * II;
Y = S - e * c.transpose();
Zmindiag = ((PR * Y.transpose()).diagonal()).array() /
((P.array() * P.array()).rowwise().sum()).array();
for (int i = 0; i < n; i++)
if (Zmindiag(i) < 0) Zmindiag(i) = 0;
Z = Zmindiag.asDiagonal();
E = Y - Z * PR;
err = (E - E_old).norm();
E_old = E;
cnt++;
}
t = -R * c;
return true;
}
void RunTimeStep(T* mSys, const int frame) {
if (frame % 20 == 0) {
water_generator->addPerturbedVolumeMixture(R3(0, .4, 9.5), I3(10, 1, 10), R3(.05, .05, 0), R3(0, -5, 0));
}
//Wheel->SetPos(Vector(0, -6, 4) );
//Wheel->SetPos_dt(Vector(0, 0, 0) );
//Wheel->SetWvel_loc(Vector(Wheel->GetWvel_loc().x, 0, 0) );
}
int main() {
Nef_polyhedron N1(Plane_3( 1, 0, 0,-1),Nef_polyhedron::INCLUDED);
Nef_polyhedron N2(Plane_3(-1, 0, 0,-1),Nef_polyhedron::INCLUDED);
Nef_polyhedron N3(Plane_3( 0, 1, 0,-1),Nef_polyhedron::INCLUDED);
Nef_polyhedron N4(Plane_3( 0,-1, 0,-1),Nef_polyhedron::INCLUDED);
Nef_polyhedron N5(Plane_3( 0, 0, 1,-1),Nef_polyhedron::INCLUDED);
Nef_polyhedron N6(Plane_3( 0, 0,-1,-1),Nef_polyhedron::INCLUDED);
Nef_polyhedron I1(!N1+!N2);
Nef_polyhedron I2(N3-!N4);
Nef_polyhedron I3(N5^N6);
Nef_polyhedron Cube1(I2 *!I1);
Cube1 *= !I3;
Nef_polyhedron Cube2 = N1 * N2 * N3 * N4 * N5 * N6;
CGAL_assertion (Cube1 == Cube2);
return 0;
}
void RunTimeStep(T* mSys, const int frame) {
if (mSys->GetNbodies() < max_particles) {
if (frame % int(100*particle_radius/.2) == 0) {
layer_gen->addPerturbedVolumeMixture(R3(0, 0, 0), I3(100, 1, 100), R3(.1, 0, .1), R3(0, -5, 0));
}
}
ang -= CH_C_PI * timestep / 2.0;
if (ang <= 0) {
ang = 2 * CH_C_PI;
}
Quaternion q1;
q1.Q_from_AngY(ang);
spinner->SetPos(Vector(0, container_height - container_size.y + 2, 0));
spinner->SetPos_dt(Vector(0, 0, 0));
//spinner->SetRot(q1);
//spinner->SetWvel_loc(Vector(0, -CH_C_PI / 2.0, 0));
}
void DecoderFastRatio::decodeFrames(cv::Mat &up, cv::Mat &vp, cv::Mat &mask, cv::Mat &shading){
const float pi = M_PI;
cv::Mat_<float> I1(frames[0]);
cv::Mat_<float> I2(frames[1]);
cv::Mat_<float> I3(frames[2]);
// cvtools::writeMat(I1, "I1.mat");
// cvtools::writeMat(I2, "I2.mat");
// cvtools::writeMat(I3, "I3.mat");
up = (I3-I1)/(I2-I1);
up = (up+1.0)/2.0;
// cvtools::writeMat(frames[0], "frames[0].mat");
// cvtools::writeMat(frames[1], "frames[1].mat");
// cvtools::writeMat(frames[2], "frames[2].mat");
up *= screenCols;
// cv::Mat upCopy = up.clone();
// cv::bilateralFilter(upCopy, up, 7, 500, 400);
cv::GaussianBlur(up, up, cv::Size(0,0), 3, 3);
shading = frames[1];
// Create mask from modulation image and erode
mask.create(shading.size(), cv::DataType<bool>::type);
// mask.setTo(true);
mask = (shading > 10000) & (shading < 65000) & (up <= screenCols) & (up >= 0);
// cv::Mat edges;
// cv::Sobel(up, edges, -1, 1, 1, 7);
// edges = abs(edges) < 500;
// cv::erode(edges, edges, cv::Mat());
// mask = mask & edges;
}
int main(int argc, char* argv[]) {
//int threads = 8;
string solver = "APGD";
if (argc > 1) {
solver = argv[1];
max_particles = atoi(argv[2]);
data_folder = argv[3];
//threads = (atoi(argv[1]));
}
if (argc == 5) {
particle_radius = atof(argv[4]);
}
omp_set_num_threads(1);
//=========================================================================================================
ch_system * system_gpu = new ch_system;
system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);
#ifndef USEGPU
if (solver == "APGD") {
system_gpu->SetLcpSolverType(ChSystem::LCP_ITERATIVE_APGD);
} else if (solver == "JACOBI") {
system_gpu->SetLcpSolverType(ChSystem::LCP_ITERATIVE_JACOBI);
//((ChLcpIterativeSolver*) system_gpu->GetLcpSolverSpeed())->SetOmega(.5);
//((ChLcpIterativeSolver*) system_gpu->GetLcpSolverSpeed())->SetSharpnessLambda(.5);
} else if (solver == "SOR") {
system_gpu->SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
}
#endif
((ChLcpIterativeSolver*) system_gpu->GetLcpSolverSpeed())->SetRecordViolation(true);
//=========================================================================================================
system_gpu->SetParallelThreadNumber(1);
system_gpu->SetMaxiter(max_iter);
system_gpu->SetIterLCPmaxItersSpeed(max_iter);
system_gpu->SetTol(tolerance);
system_gpu->SetTolSpeeds(tolerance);
system_gpu->SetMaxPenetrationRecoverySpeed(30);
#ifdef USEGPU
//((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIteration(max_iteration);
((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationNormal(max_iter/2);
((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSliding(max_iter/2);
((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSpinning(0);
((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationBilateral(0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(tolerance);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance( 0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(5);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(APGDRS);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius * .01);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBinsPerAxis(I3(30, 30, 30));
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBodyPerBin(100, 50);
((ch_system*) system_gpu)->SetAABB(R3(-6, -6, -6), R3(6, 6, 6));
#endif
system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
system_gpu->SetStep(timestep);
//=========================================================================================================
ChSharedBodyPtr L = ChSharedBodyPtr(new ch_body);
ChSharedBodyPtr R = ChSharedBodyPtr(new ch_body);
ChSharedBodyPtr F = ChSharedBodyPtr(new ch_body);
ChSharedBodyPtr B = ChSharedBodyPtr(new ch_body);
ChSharedBodyPtr Bottom = ChSharedBodyPtr(new ch_body);
ChSharedBodyPtr Top = ChSharedBodyPtr(new ch_body);
ChSharedBodyPtr Tube = ChSharedBodyPtr(new ch_body);
ChSharedPtr<ChMaterialSurface> material;
material = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material->SetFriction(.4);
material->SetRollingFriction(0);
material->SetSpinningFriction(0);
material->SetCompliance(0);
material->SetCohesion(0);
InitObject(Bottom, 100000, Vector(0, container_height - container_size.y, 0), Quaternion(1, 0, 0, 0), material, true, true, 1, 1);
AddCollisionGeometry(Bottom, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
FinalizeObject(Bottom, (ch_system *) system_gpu);
ChSharedBodyPtr Ring;
for (int i = 0; i < 360; i += 5) {
real angle = i * PI / 180.0;
real x = cos(angle) * 5;
real z = sin(angle) * 5;
Quaternion q;
q.Q_from_AngAxis(-angle, Vector(0, 1, 0));
Ring = ChSharedBodyPtr(new ch_body);
InitObject(Ring, 100000, Vector(x, container_height - 3, z), q, material, true, true, 1, 1);
AddCollisionGeometry(Ring, BOX, Vector(.25, 3, .25), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
FinalizeObject(Ring, (ch_system *) system_gpu);
}
spinner = ChSharedBodyPtr(new ch_body);
InitObject(spinner, 100000, Vector(0, container_height - container_size.y + 2, 0), Quaternion(1, 0, 0, 0), material, true, false, 1, 1);
real spinner_h = .5;
AddCollisionGeometry(spinner, CYLINDER, Vector(.5, .4, .5), Vector(0, 0, 0), chrono::Q_from_AngAxis(0, ChVector<>(0, 0, 1)));
AddCollisionGeometry(spinner, BOX, Vector(container_thickness / 15.0, spinner_h, 1.5), Vector(0, 0, 1.75), chrono::Q_from_AngAxis(CH_C_PI / 4.0, ChVector<>(0, 0, 1)));
AddCollisionGeometry(spinner, BOX, Vector(container_thickness / 15.0, spinner_h, 1.5), Vector(0, 0, -1.75), chrono::Q_from_AngAxis(-CH_C_PI / 4.0, ChVector<>(0, 0, 1)));
AddCollisionGeometry(spinner, BOX, Vector(1.5, spinner_h, container_thickness / 15.0), Vector(1.75, 0, 0), chrono::Q_from_AngAxis(CH_C_PI / 4.0, ChVector<>(1, 0, 0)));
AddCollisionGeometry(spinner, BOX, Vector(1.5, spinner_h, container_thickness / 15.0), Vector(-1.75, 0, 0), chrono::Q_from_AngAxis(-CH_C_PI / 4.0, ChVector<>(1, 0, 0)));
FinalizeObject(spinner, (ch_system *) system_gpu);
#ifdef USEGPU
layer_gen = new ParticleGenerator<ch_system>((ch_system *) system_gpu, true);
#else
layer_gen = new ParticleGenerator<ch_system>((ch_system *) system_gpu, false);
#endif
layer_gen->SetDensity(1000);
layer_gen->SetRadius(R3(particle_radius));
layer_gen->material->SetFriction(.5);
layer_gen->material->SetCohesion(particle_cohesion);
#ifdef USEGPU
layer_gen->material->SetCompliance(0);
#else
layer_gen->material->SetCompliance(0);
#endif
layer_gen->material->SetSpinningFriction(0);
layer_gen->material->SetRollingFriction(0);
layer_gen->SetRadius(R3(particle_radius, particle_radius * 1.1, particle_radius));
layer_gen->SetCylinderRadius(4.5);
layer_gen->SetNormalDistribution(particle_radius, .01);
layer_gen->AddMixtureType(MIX_SPHERE);
layer_gen->AddMixtureType(MIX_ELLIPSOID);
//layer_gen->AddMixtureType(MIX_CYLINDER);
layer_gen->AddMixtureType(MIX_CUBE);
//layer_gen->AddMixtureType(MIX_CONE);
//=========================================================================================================
////Rendering specific stuff:
// ChOpenGLManager * window_manager = new ChOpenGLManager();
// ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
//
// //openGLView.render_camera->camera_position = glm::vec3(0, -5, -10);
// // openGLView.render_camera->camera_look_at = glm::vec3(0, -5, 0);
// // openGLView.render_camera->camera_scale = .1;
// openGLView.SetCustomCallback(RunTimeStep);
// openGLView.StartSpinning(window_manager);
// window_manager->CallGlutMainLoop();
//=========================================================================================================
stringstream s1;
s1 << data_folder << "/residual.txt";
CSVGen csv_output;
csv_output.OpenFile(s1.str().c_str());
int file = 0;
for (int i = 0; i < num_steps; i++) {
system_gpu->DoStepDynamics(timestep);
double TIME = system_gpu->GetChTime();
double STEP = system_gpu->GetTimerStep();
double BROD = system_gpu->GetTimerCollisionBroad();
double NARR = system_gpu->GetTimerCollisionNarrow();
double LCP = system_gpu->GetTimerLcp();
double UPDT = system_gpu->GetTimerUpdate();
std::vector<double> violation = ((ChLcpIterativeSolver*) system_gpu->GetLcpSolverSpeed())->GetViolationHistory();
int REQ_ITS = violation.size();
double RESID = 0;
if (REQ_ITS != 0) {
RESID = violation.at(violation.size() - 1);
}
int BODS = system_gpu->GetNbodies();
int CNTC = system_gpu->GetNcontacts();
printf("%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7d|%7d|%7d|%7.4f\n", TIME, STEP, BROD, NARR, LCP, UPDT, BODS, CNTC, REQ_ITS, RESID);
int save_every = 1.0 / timestep / 60.0; //save data every n steps
// if (i % save_every == 0) {
// stringstream ss;
// cout << "Frame: " << file << endl;
// ss << data_folder << "/" << file << ".txt";
// //DumpAllObjects(system_gpu, ss.str(), ",", true);
// DumpAllObjectsWithGeometryPovray(system_gpu, ss.str());
// file++;
//
// }
csv_output << TIME;
csv_output << STEP;
csv_output << BROD;
csv_output << NARR;
csv_output << LCP;
csv_output << UPDT;
csv_output << BODS;
csv_output << CNTC;
csv_output << REQ_ITS;
csv_output << RESID;
csv_output.Endline();
RunTimeStep(system_gpu, i);
}
csv_output.CloseFile();
}
I3 QtBox::getIndices()
{
return I3(idxX,idxY,invLevel);
}
int main(int argc, char* argv[]) {
//omp_set_num_threads(8);
if (argc == 4) {
cohesion_water = atof(argv[1]);
cohesion_goop = atof(argv[2]);
data_folder = argv[3];
cout << cohesion_water << " " << cohesion_goop << " " << data_folder << endl;
}
//=========================================================================================================
ChSystemParallelDVI * system_gpu = new ChSystemParallelDVI;
system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);
//=========================================================================================================
//system_gpu->SetMaxiter(max_iter);
//system_gpu->SetIterLCPmaxItersSpeed(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationNormal(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSliding(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSpinning(0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationBilateral(max_iter * 2);
system_gpu->SetTol(.5);
system_gpu->SetTolSpeeds(.5);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(.5);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance(.0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(1);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(APGDRS);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetWarmStart(false);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius * .05);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBinsPerAxis(I3(100, 100, 100));
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBodyPerBin(200, 100);
system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
system_gpu->SetStep(timestep);
//((ChSystemParallel*) system_gpu)->SetAABB(R3(-6, -6, -6), R3(6, 6, 6));
ChSharedBodyPtr L = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr R = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr F = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr B = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr Bottom = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr Top = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr Float = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedPtr<ChMaterialSurface> material;
material = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material->SetFriction(container_friction);
//material->SetRollingFriction(.5);
//material->SetSpinningFriction(.5);
material->SetCompliance(0);
material->SetCohesion(-1000);
InitObject(L, 100000, Vector(-container_size.x + container_thickness, container_height - container_thickness, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(R, 100000, Vector(container_size.x - container_thickness, container_height - container_thickness, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(F, 100000, Vector(0, container_height - container_thickness, -container_size.z + container_thickness), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(B, 100000, Vector(0, container_height - container_thickness, container_size.z - container_thickness), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(Bottom, 100000, Vector(0, container_height - container_size.y, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(Top, 100000, Vector(0, container_height + container_size.y, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
Quaternion qqq;
qqq.Q_from_AngZ(15);
InitObject(Float, 100000, Vector(0, -3, 0), qqq, material, true, true, -20, -20);
AddCollisionGeometry(L, BOX, Vector(container_thickness, container_size.y, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(R, BOX, Vector(container_thickness, container_size.y, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(F, BOX, Vector(container_size.x, container_size.y, container_thickness), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(B, BOX, Vector(container_size.x, container_size.y, container_thickness), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Bottom, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
//AddCollisionGeometry(Top, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
//AddCollisionGeometry(Float, BOX, Vector(1, 1, 1), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
FinalizeObject(L, (ChSystemParallel *) system_gpu);
FinalizeObject(R, (ChSystemParallel *) system_gpu);
FinalizeObject(F, (ChSystemParallel *) system_gpu);
FinalizeObject(B, (ChSystemParallel *) system_gpu);
FinalizeObject(Bottom, (ChSystemParallel *) system_gpu);
//FinalizeObject(Float, (ChSystemParallel *) system_gpu);
Quaternion quat;
quat.Q_from_AngAxis(90, Vector(0, 1, 0));
ChSharedPtr<ChMaterialSurface> material_spout;
material_spout = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material_spout->SetFriction(0);
//material->SetRollingFriction(.5);
//material->SetSpinningFriction(.5);
material_spout->SetCompliance(0);
material_spout->SetCohesion(-1000);
ChSharedBodyPtr Spout = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(Spout, 100000, Vector(0, 0, 8), Quaternion(1, 0, 0, 0), material_spout, true, true, -20, -20);
real width = 1;
real thick = .1;
real depth = 2;
real height = .5;
AddCollisionGeometry(Spout, BOX, Vector(width, thick, depth), Vector(0, height, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Spout, BOX, Vector(width, thick, depth), Vector(0, -height, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Spout, BOX, Vector(thick, height, depth), Vector(width, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Spout, BOX, Vector(thick, height, depth), Vector(-width, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Spout, BOX, Vector(width, height, thick), Vector(0, 0, depth), Quaternion(1, 0, 0, 0));
FinalizeObject(Spout, (ChSystemParallel *) system_gpu);
real wheel_rad = 3;
real paddle_len = .5;
real thickness = .05;
real wheel_width = 1;
Wheel = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(Wheel, 100, Vector(0, -6, 4), Quaternion(1, 0, 0, 0), material, true, false, -20, -20);
AddCollisionGeometry(Wheel, CYLINDER, Vector(wheel_rad, wheel_width, wheel_rad), Vector(0, 0, 0), chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_Z));
AddCollisionGeometry(Wheel, CYLINDER, Vector(wheel_rad + paddle_len * 2, thickness, wheel_rad + paddle_len * 2), Vector(-wheel_width, 0, 0), chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_Z));
AddCollisionGeometry(Wheel, CYLINDER, Vector(wheel_rad + paddle_len * 2, thickness, wheel_rad + paddle_len * 2), Vector(wheel_width, 0, 0), chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_Z));
for (int i = 0; i < 360; i += 30) {
real angle = i * PI / 180.0;
real x = cos(angle) * (wheel_rad + paddle_len);
real z = sin(angle) * (wheel_rad + paddle_len);
Quaternion q;
q.Q_from_AngAxis(-angle, Vector(1, 0, 0));
AddCollisionGeometry(Wheel, BOX, Vector(wheel_width, thickness, paddle_len), Vector(0, z, x), q);
}
FinalizeObject(Wheel, (ChSystemParallel *) system_gpu);
ChSharedPtr<ChLinkLockRevolute> my_link1(new ChLinkLockRevolute);
my_link1->Initialize(Bottom, Wheel, ChCoordsys<>(Vector(0, -6, 4), chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_Y)));
system_gpu->AddLink(my_link1);
water_generator = new ParticleGenerator<ChSystemParallel>((ChSystemParallel *) system_gpu);
water_generator->SetDensity(1000);
water_generator->SetRadius(R3(particle_radius, particle_radius, particle_radius));
water_generator->material->SetFriction(particle_friction);
water_generator->material->SetCohesion(cohesion_water);
water_generator->material->SetRollingFriction(0);
water_generator->material->SetSpinningFriction(0);
water_generator->material->SetCompliance(0);
water_generator->AddMixtureType(MIX_SPHERE);
//=========================================================================================================
////Rendering specific stuff:
// ChOpenGLManager * window_manager = new ChOpenGLManager();
// ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
// //openGLView.render_camera->camera_position = glm::vec3(0, -5, -10);
// //openGLView.render_camera->camera_look_at = glm::vec3(0, -5, 0);
// //openGLView.render_camera->camera_scale = .4;
// openGLView.SetCustomCallback(RunTimeStep);
// openGLView.StartSpinning(window_manager);
// window_manager->CallGlutMainLoop();
//=========================================================================================================
int file = 0;
for (int i = 0; i < num_steps; i++) {
system_gpu->DoStepDynamics(timestep);
double TIME = system_gpu->GetChTime();
double STEP = system_gpu->GetTimerStep();
double BROD = system_gpu->GetTimerCollisionBroad();
double NARR = system_gpu->GetTimerCollisionNarrow();
double LCP = system_gpu->GetTimerLcp();
double UPDT = system_gpu->GetTimerUpdate();
double RESID = ((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->GetResidual();
int BODS = system_gpu->GetNbodies();
int CNTC = system_gpu->GetNcontacts();
int REQ_ITS = ((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->GetTotalIterations();
printf("%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7d|%7d|%7d|%7.4f\n", TIME, STEP, BROD, NARR, LCP, UPDT, BODS, CNTC, REQ_ITS, RESID);
int save_every = 1.0 / timestep / 60.0; //save data every n steps
if (i % save_every == 0) {
stringstream ss;
cout << "Frame: " << file << endl;
ss << data_folder << "/" << file << ".txt";
DumpAllObjectsWithGeometryPovray(system_gpu, ss.str());
file++;
}
RunTimeStep(system_gpu, i);
}
//DumpObjects(system_gpu, "diagonal_impact_settled.txt", "\t");
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
//bridge that will transform the message (image) from ROS code back to "image" code
sensor_msgs::CvBridge bridge;
fprintf(stderr, "\n callBaack funtion \n");
//publish data (obstacle waypoints) back to the boat
ros::NodeHandle n;
//std_msgs::Float32 xWaypoint_msg; // X coordinate obstacle message
//std_msgs::Float32 yWaypoint_msg; // Y coordinate obstacle message
//publish the waypoint data
//ros::Publisher waypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//ros::Publisher Ywaypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//std::stringstream ss;
/***********************************************************************/
//live image coming streamed straight from the boat's camera
IplImage* boatFront = bridge.imgMsgToCv(msg, "bgr8");
IplImage* backUpImage = bridge.imgMsgToCv(msg, "bgr8");
boatFront->origin = IPL_ORIGIN_TL; //sets image origin to top left corner
//Crop the image to the ROI
cvSetImageROI(boatFront, cvRect(0,0,boatFront->height/0.5,boatFront->width/1.83));
int X = boatFront->height;
int Y = boatFront->width;
/***********************************************************************/
//boat's edge distance from the camera. This is used for visual calibration
//to know the distance from the boat to the nearest obstacles.
//With respect to the mounted camera, distance is 21 inches (0.5334 m) side to side
//and 15 inches (0.381 m).
//float boatFrontDistance = 0.381; //distance in meters
//float boatSideDistance = 0.5334; //distance in meters
// These variables tell the distance from the center bottom of the image
// (the camera) to the square surrounding a the obstacle
float xObstacleDistance = 0.0;
float yObstacleDistance = 0.0;
float obstacleDistance = 0.0;
int pixelsNumber = 6; //number of pixels for an n x n matrix and # of neighbors
const int arraySize = pixelsNumber;
const int threeArraySize = pixelsNumber;
//if n gets changed, then the algorithm might have to be
//recalibrated. Try to keep it constant
//these variables are used for the k nearest neighbors
//int accuracy;
//reponses for each of the classifications
float responseWaterH, responseWaterS, responseWaterV;
float responseGroundH, responseGroundS, responseGroundV;
//float responseSkyH, responseSkyS, responseSkyV;
float averageHue = 0.0;
float averageSat = 0.0;
float averageVal = 0.0;
CvMat* trainClasses = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClasses2 = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
for (int i = 0; i < pixelsNumber/2; i++)
{
cvmSet(trainClasses, i,0,1);
cvmSet(trainClasses2, i,0,1);
}
for (int i = pixelsNumber/2; i < pixelsNumber; i++)
{
cvmSet(trainClasses, i,0,2);
cvmSet(trainClasses2, i,0,2);
}
//for (int i =0; i<pixelsNumber;i++)
//{
// cout << cvmGet(trainClasses,i,0);
// cout << cvmGet(trainClasses2,i,0);
//}
//CvMat sample = cvMat( 1, 2, CV_32FC1, _sample );
//used with the classifier
CvMat* nearestWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* nearestSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//Distance
//CvMat* distanceWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
//CvMat* distanceSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//these variables are use to traverse the picture by blocks of n x n pixels at
//a time.
//Index(0,0) does not exist, so make sure kj and ki start from 1 (in the
//right way, of course)
//x and y are the dimensions of the local patch of pixels
int x = (boatFront->height)/2.5 + pixelsNumber + 99;
int y = pixelsNumber-1;
int ix = 0;
int iy = 0;
int skyX = 0;
int skyY = 0;
//M controls the x axis (up and down); N controls the y axis (left and
//right)
int Mw = -550;
int Nw = 1300;
int Mg = -350;
int Ng = 700;
int row1 = 0;
int column1 = 0;
int row2 = 0;
int column2 = 0;
//ground sample
CvMat* groundTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* groundTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* groundTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//water sample
CvMat* waterTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//n x n sample patch taken from the picture
CvMat* sampleHue = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleSat = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleVal = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* resampleHue = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* resampleSat = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* resampleVal = cvCreateMat(arraySize,arraySize,CV_32FC1);
//sky training sample
CvMat* skyTrainingHue = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingSat = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingVal = cvCreateMat(arraySize,arraySize,CV_32FC1);
//initialize each matrix element to zero for ease of use
cvZero(groundTrainingHue);
cvZero(groundTrainingSat);
cvZero(groundTrainingVal);
cvZero(waterTrainingHue);
cvZero(waterTrainingSat);
cvZero(waterTrainingVal);
cvZero(sampleHue);
cvZero(sampleSat);
cvZero(sampleVal);
cvZero(resampleHue);
cvZero(resampleSat);
cvZero(resampleVal);
cvZero(skyTrainingHue);
cvZero(skyTrainingSat);
cvZero(skyTrainingVal);
//Stores the votes for each channel (whether it belongs to water or not
//1 is part of water, 0 not part of water
//if sum of votes is bigger than 1/2 the number of elements, then it belongs to water
int votesSum = 0;
int comparator[3]; //used when only three votes are needed
//int comparatorTwo [3][3]; //used when six votes are needed
//initial sum of votes is zero
//Error if initialize both matrices inside a single for loop. Dont know why
// for(int i = 0; i < 3; i++)
//{
//comparator[i] = 0;
// for(int j = 0; j < 3; j++)
// {
// comparatorTwo[i][j] = 0;
// }
//}
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
/***********************************************************************/
//Convert from RGB to HSV to control the brightness of the objects.
//work with reflexion
/*Sky recognition. Might be useful for detecting reflexion on the water. If
the sky is detected, and the reflection has the same characteristics of
something below the horizon, that "something" might be water. Assume sky
wont go below the horizon*/
//convert from RGB to HSV
cvCvtColor(boatFront, boatFront, CV_BGR2HSV);
cvCvtColor(backUpImage, backUpImage, CV_BGR2HSV);
HsvImage I(boatFront);
HsvImage IBackUp(backUpImage);
fprintf(stderr,"\n About to do Sky detection\n");
//Sky detection
for (int i=0; i<boatFront->height/3;i++)
{
for (int j=0; j<boatFront->width;j++)
{
//if something is bright enough, consider it sky and store the
//value. HSV values go from 0 to 180 ... RGB goes from 0 to 255
if (((I[i][j].v >= 180) && (I[i][j].s <= 16)))
// && ((I[i][j].h >=10)))) //&& (I[i][j].h <= 144))))
{
//The HSV values vary between 0 and 1
cvmSet(skyTrainingHue,skyX,skyY,I[i][j].h);
cvmSet(skyTrainingSat,skyX,skyY,I[i][j].s);
cvmSet(skyTrainingVal,skyX,skyY,I[i][j].v);
I[i][j].h = 0.3*180; //H (color)
I[i][j].s = 0.3*180; //S (color intensity)
I[i][j].v = 0.6*180; //V (brightness)
if (skyY == pixelsNumber-1)
{
if (skyX == pixelsNumber-1)
skyX = 1;
else
skyX = skyX + 1;
skyY = 1;
}
else
skyY = skyY + 1;
}
}
}
/***********************************************************************/
//offline input pictures. Samples of water properties are taken from these
//pictures to get a range of values for H, S, V that will be stored into a
//pre-defined classifier
IplImage* imageSample1 = cvLoadImage("bigObstacle.jpg");
cvSetImageROI(imageSample1, cvRect(0,0,imageSample1->height/0.5,imageSample1->width/1.83));
cvCvtColor(imageSample1, imageSample1, CV_BGR2HSV);
HsvImage I1(imageSample1);
IplImage* imageSample2 = cvLoadImage("bigObstacle2.jpg");
cvSetImageROI(imageSample2, cvRect(0,0,imageSample2->height/0.5,imageSample2->width/1.83));
cvCvtColor(imageSample2, imageSample2, CV_BGR2HSV);
HsvImage I2(imageSample2);
IplImage* imageSample3 = cvLoadImage("bigObstacle3.jpg");
cvSetImageROI(imageSample3, cvRect(0,0,imageSample3->height/0.5,imageSample3->width/1.83));
cvCvtColor(imageSample3, imageSample3, CV_BGR2HSV);
HsvImage I3(imageSample3);
IplImage* imageSample4 = cvLoadImage("river.jpg");
cvSetImageROI(imageSample4, cvRect(0,0,imageSample4->height/0.5,imageSample4->width/1.83));
cvCvtColor(imageSample4, imageSample4, CV_BGR2HSV);
HsvImage I4(imageSample4);
IplImage* imageSample5 = cvLoadImage("river2.jpg");
cvSetImageROI(imageSample5, cvRect(0,0,imageSample5->height/0.5,imageSample5->width/1.83));
cvCvtColor(imageSample5, imageSample5, CV_BGR2HSV);
HsvImage I5(imageSample5);
IplImage* imageSample6 = cvLoadImage("roundObstacle4.jpg");
cvSetImageROI(imageSample6, cvRect(0,0,imageSample6->height/0.5,imageSample6->width/1.83));
cvCvtColor(imageSample6, imageSample6, CV_BGR2HSV);
HsvImage I6(imageSample6);
IplImage* imageSample7 = cvLoadImage("farm.jpg");
cvSetImageROI(imageSample7, cvRect(0,0,imageSample7->height/0.5,imageSample7->width/1.83));
cvCvtColor(imageSample7, imageSample7, CV_BGR2HSV);
HsvImage I7(imageSample7);
IplImage* imageSample8 = cvLoadImage("bigObstacle4.jpg");
cvSetImageROI(imageSample8, cvRect(0,0,imageSample8->height/0.5,imageSample8->width/1.83));
cvCvtColor(imageSample8, imageSample8, CV_BGR2HSV);
HsvImage I8(imageSample8);
IplImage* imageSample9 = cvLoadImage("roundObstacle6.jpg");
cvSetImageROI(imageSample9, cvRect(0,0,imageSample9->height/0.5,imageSample9->width/1.83));
cvCvtColor(imageSample9, imageSample9, CV_BGR2HSV);
HsvImage I9(imageSample9);
IplImage* imageSample10 = cvLoadImage("roundObstacle.jpg");
cvSetImageROI(imageSample10, cvRect(0,0,imageSample10->height/0.5,imageSample10->width/1.83));
cvCvtColor(imageSample10, imageSample10, CV_BGR2HSV);
HsvImage I10(imageSample10);
fprintf(stderr,"\n Grab water samples\n");
//grab water samples from each picture
for (int i=0; i < threeArraySize; i++)
{
fprintf(stderr,"\n patch is pink (this is for me to know where the ground patch sample is\n");
for (int j=0; j < arraySize; j++)
{
row1 = ceil(X/1.2866)+ceil(X/5.237)+i+Mw;
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = ceil(Y/7.0755)+ceil(Y/21.01622)+j+Nw;
if (column1 > Y-1)
column1 = Y-1;
averageHue = (I1[row1][column1].h + I2[row1][column1].h + I3[row1][column1].h + I4[row1][column1].h + I5[row1][column1].h +
I6[row1][column1].h + I7[row1][column1].h + I8[row1][column1].h + I9[row1][column1].h + I10[row1][column1].h) / 10;
averageSat = (I1[row1][column1].s + I2[row1][column1].s + I3[row1][column1].s + I4[row1][column1].s + I5[row1][column1].s +
I6[row1][column1].s + I7[row1][column1].s + I8[row1][column1].s + I9[row1][column1].s + I10[row1][column1].s) / 10;
averageVal = (I1[row1][column1].v + I2[row1][column1].v + I3[row1][column1].v + I4[row1][column1].v + I5[row1][column1].v +
I6[row1][column1].v + I7[row1][column1].v + I8[row1][column1].v + I9[row1][column1].v + I10[row1][column1].v) / 10;
fprintf(stderr,"\n water patch sample (n X n matrix)\n");
cvmSet(waterTrainingHue,i,j,averageHue);
cvmSet(waterTrainingSat,i,j,averageSat);
cvmSet(waterTrainingVal,i,j,averageVal);
fprintf(stderr,"\n patch is red (this is for me to know where the ground patch sample is\n");
//I[row1][column1].h = 0;
//I[row1][column1].s = 255;
//I[row1][column1].v = 255;
}
}
fprintf(stderr,"\n Order water samples in ascending\n");
//order the water samples in ascending order on order to know a range
cvSort(waterTrainingHue, waterTrainingHue, CV_SORT_ASCENDING);
cvSort(waterTrainingSat, waterTrainingSat, CV_SORT_ASCENDING);
cvSort(waterTrainingVal, waterTrainingVal, CV_SORT_ASCENDING);
// find the maximum and minimum values in the array to create a range
int maxH = cvmGet(waterTrainingHue,0,0);
int maxS = cvmGet(waterTrainingSat,0,0);
int maxV = cvmGet(waterTrainingVal,0,0);
int minH = cvmGet(waterTrainingHue,0,0);
int minS = cvmGet(waterTrainingSat,0,0);
int minV = cvmGet(waterTrainingVal,0,0);
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
if (cvmGet(waterTrainingHue,i,j) > maxH)
maxH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) > maxS)
maxS = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingVal,i,j) > maxV)
maxV = cvmGet(waterTrainingVal,i,j);
if (cvmGet(waterTrainingHue,i,j) < minH)
minH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) < minS)
minS = cvmGet(waterTrainingSat,i,j);
if (cvmGet(waterTrainingVal,i,j) < minV)
minV = cvmGet(waterTrainingVal,i,j);
}
}
/***********************************************************************/
//Grab a random patch of water below the horizon and compare every other
//pixel against it
//The results of the water detection depend on where in the picture the
//training samples are located. Maybe adding more training samples will
//help improve this?
fprintf(stderr,"\n Random patch\n");
/*for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
row2 = ceil(X/4.7291)+ceil(X/8.3176)+i+Mg;
column2 = ceil(Y/7.78378)+ceil(Y/16.54468)+j+Ng;
//ground patch sample (n X n matrix)
//Detecting the horizon in the picture might be an excellent visual aid to
//choose where (above the horizon) you can take a ground training(1:3*n,1:n)g sample
//from. The ground pixel sample can be at a constant distance from the
//horizon
cvmSet(groundTrainingHue,i,j,I[row2][column2].h);
cvmSet(groundTrainingSat,i,j,I[row2][column2].s);
cvmSet(groundTrainingVal,i,j,I[row2][column2].v);
//patch is red (this is for me to know where the ground patch sample is)
I[row2][column2].h = 60;
I[row2][column2].s = 180;
I[row2][column2].v = 90;
}
}
//order the water samples in ascending order on order to know a range
cvSort(groundTrainingHue, groundTrainingHue, CV_SORT_ASCENDING);
cvSort(groundTrainingSat, groundTrainingSat, CV_SORT_ASCENDING);
cvSort(groundTrainingVal, groundTrainingVal, CV_SORT_ASCENDING);
*/
// Main loop. It traverses through the picture
//skyX = 0;
//skyY = 0;
//The distance formula calculated by plotting points is given by:
/*********** distance = 0.0006994144*(1.011716711^x) *****************/
//cout << "Distance: " << distancePixels << endl;
while (x < boatFront->height/1.158)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<pixelsNumber;i++)
{
cvmSet(sampleHue,0,i,I[x][y].h);
cvmSet(sampleSat,0,i,I[x][y].s);
cvmSet(sampleVal,0,i,I[x][y].v);
}
//Find the shortest distance between a pixel and the neighbors from each of
//the training samples (sort of inefficient, but might do the job...sometimes)
//if (ix == pixelsNumber-1)
//{
//HSV for water sample
// learn classifier
//CvKNearest knn(trainData, trainClasses, 0, false, itemsNumber);
//CvKNearest knnWaterHue(waterTrainingHue, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnWaterSat(waterTrainingSat, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnWaterVal(waterTrainingVal, trainClasses, 0, false, pixelsNumber);
//HSV for ground sample
//CvKNearest knnGroundHue(groundTrainingHue, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundSat(groundTrainingSat, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundVal(groundTrainingVal, trainClasses2, 0, false, pixelsNumber);
//HSV for sky sample
//if (cvmGet(skyTrainingHue,0,0)!=0.0 && cvmGet(skyTrainingSat,0,0)!=0.0 && cvmGet(skyTrainingVal,0,0)!=0.0)
//{
// CvKNearest knnSkyHue(skyTrainingHue, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnSkySat(skyTrainingSat, trainClasses, 0, false, pixelsNumber);
//CvKNearest knnSkyVal(skyTrainingVal, trainClasses, 0, false, pixelsNumber);
//}
//scan nearest neighbors to each pixel
//responseWaterH = knnWaterHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestWaterH,0);
//responseWaterS = knnWaterSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestWaterS,0);
//responseWaterV = knnWaterVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestWaterV,0);
//responseGroundH = knnGroundHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestGroundH,0);
//responseGroundS = knnGroundSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestGroundS,0);
//responseGroundV = knnGroundVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestGroundV,0);
for (int i=0;i<pixelsNumber;i++)
{
for (int j=0;j<pixelsNumber;j++)
{
if ((minH <= cvmGet(sampleHue,0,j)) || (maxH >= cvmGet(sampleHue,0,j)))
//mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if (((minS <= cvmGet(sampleSat,0,j)) || (maxS <= cvmGet(sampleSat,0,j))))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((minV <= cvmGet(sampleVal,0,j)) || (maxV <= cvmGet(sampleVal,0,j)))
//mark water samples as green
comparator[2] = 1;
else
comparator[2] = 0;
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
//sky detection
if (votesSum > 1) //&& ((sampleSat[i][j] - sampleVal[i][j]) <= 0.1*180)
{
// classify pixel as water
I[x-pixelsNumber+i][y-pixelsNumber+j].h = 0;
I[x-pixelsNumber+i][y-pixelsNumber+j].s = 255;
I[x-pixelsNumber+i][y-pixelsNumber+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
y = y + pixelsNumber-1;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
x = x + pixelsNumber-1;
y = pixelsNumber-1;
}
//ix = 0;
}
//traverse through the image one more time, divide the image in grids of
// 500x500 pixels, and see how many pixels of water are in each grid. If
// most of the pixels are labeled water, then mark all the other pixels
// as water as well
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
int xDivisor = 20;
int yDivisor = 20;
votesSum = 0;
column1 = 0;
row1 = 0;
x = ceil(boatFront->height/2.5);
obstacleDistance = x;
y = 0;
int counter = 0;
while (x < boatFront->height/1.2)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i < ceil(boatFront->height/xDivisor); i++)
{
for(int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
cvmSet(resampleHue,i,j,I[x+i][y+j].h);
cvmSet(resampleSat,i,j,I[x+i][y+j].s);
cvmSet(resampleVal,i,j,I[x+i][y+j].v);
if(cvmGet(resampleHue,i,j)==0 && cvmGet(resampleSat,i,j)==255 && cvmGet(resampleVal,i,j)==255)
{
votesSum++;
}
}
}
if (votesSum > ((boatFront->height/xDivisor)*(boatFront->width/yDivisor)*(8.9/9)))
{
// if bigger than 4/5 the total number of pixels in a square, then consider the entire thing as water
// We might need to use other smaller quantities (like 5/6 maybe?)
for (int i = 0; i < ceil(boatFront->height/xDivisor);i++)
{
for (int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
I[row1][column1].h = 0;
I[row1][column1].s = 255;
I[row1][column1].v = 255;
}
}
}
else
{
// If not water, eliminate all red pixels and turn those pixels
// back to the original color. These pixels shall, then, be marked
// as obstacles
for (int i = 0; i < ceil(boatFront->height/xDivisor);i++)
{
for (int j = 0; j < ceil(boatFront->width/yDivisor); j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
//the darker the color, the closer the object to the boat
I[row1][column1].h = 128;
I[row1][column1].s = 255;
I[row1][column1].v = 255 - counter;
//I[row1][column1].h = IBackUp[row1][column1].h;
//I[row1][column1].s = IBackUp[row1][column1].s;
//I[row1][column1].v = IBackUp[row1][column1].v;
//counter = counter + 20;
}
}
//The distance formula calculated by plotting points is given by:
/*********** distance = (1.76e-11)*pow(pixels,3.99) *****************/
/*********** pixel = (513.9332077469)pow(distance,0.240675506 *****************/
// Convert from pixel distance to normal distance in meters
if(obstacleDistance > sqrt(pow(xObstacleDistance,2) + pow(yObstacleDistance,2)))
{
// x,y coordinates of the obstacle
xObstacleDistance = (1.76e-11)*pow(((boatFront->height/xDivisor)+x)/2, 3.99) ;
yObstacleDistance = (1.76e-11)*pow(((boatFront->width/yDivisor)+y)/2, 3.99);
//xWaypoint_msg = xObstacleDistance;
//yWaypoint_msg = yObstacleDistance;
//publish position data
//waypoint_info_pub.publish(xWaypoint_msg);
//waypoint_info_pub.publish(yWaypoint_msg);
//ROS_INFO("Obstacle coordinates: X = %f meters, Y = %f meters", xObstacleDistance, yObstacleDistance);
obstacleDistance = sqrt(pow(xObstacleDistance,2) + pow(yObstacleDistance,2));
//ROS_INFO("Obstacle distance from: %f", obstacleDistance);
}
//cout << "Distance to Obstacle is: " << obstacleDistance << endl << endl;
}
y = y + boatFront->width/xDivisor;
if (y > Y-1)
{
x = x + boatFront->height/yDivisor;
y = 0;
counter = counter + 30;
}
votesSum = 0;
}
cvCvtColor(boatFront, boatFront, CV_HSV2BGR);
cvCvtColor(backUpImage, backUpImage, CV_HSV2BGR)
/**************************************************************************/
try
{
cvShowImage("Boat Front", boatFront);
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
int main(int argc, char* argv[]) {
int threads = 8;
if (argc > 1) {
threads = (atoi(argv[1]));
}
omp_set_num_threads(threads);
//=========================================================================================================
ChSystemParallelDVI * system_gpu = new ChSystemParallelDVI;
system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);
//=========================================================================================================
system_gpu->SetParallelThreadNumber(threads);
system_gpu->SetMaxiter(max_iter);
system_gpu->SetIterLCPmaxItersSpeed(max_iter);
((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationNormal(10);
((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSliding(10);
((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSpinning(0);
((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationBilateral(10);
system_gpu->SetTol(.0001);
system_gpu->SetTolSpeeds(.0001);
system_gpu->SetMaxPenetrationRecoverySpeed(100);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(.0001);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance( 0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(1);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(APGDRS);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius * .01);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBinsPerAxis(I3(50, 50, 50));
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBodyPerBin(100, 50);
system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
system_gpu->SetStep(timestep);
((ChSystemParallel*) system_gpu)->SetAABB(R3(-6, -3, -12), R3(6, 6, 12));
//=========================================================================================================
//cout << num_per_dir.x << " " << num_per_dir.y << " " << num_per_dir.z << " " << num_per_dir.x * num_per_dir.y * num_per_dir.z << endl;
//addPerturbedLayer(R3(0, -5 +container_thickness-particle_radius.y, 0), ELLIPSOID, particle_radius, num_per_dir, R3(.01, .01, .01), 10, 1, system_gpu);
//addHCPCube(num_per_dir.x, num_per_dir.y, num_per_dir.z, 1, particle_radius.x, 1, true, 0, -6 +container_thickness+particle_radius.y, 0, 0, system_gpu);
//=========================================================================================================
ChSharedBodyPtr L = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr R = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr F = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr B = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr Bottom = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr Top = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr Tube = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedPtr<ChMaterialSurface> material;
material = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material->SetFriction(.1);
material->SetRollingFriction(0);
material->SetSpinningFriction(0);
material->SetCompliance(0);
material->SetCohesion(-100);
Quaternion q;
q.Q_from_AngX(-.1);
InitObject(L, 100000, Vector(-container_size.x + container_thickness, container_height - container_thickness, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(R, 100000, Vector(container_size.x - container_thickness, container_height - container_thickness, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(F, 100000, Vector(0, container_height - container_thickness, -container_size.z + container_thickness), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(B, 100000, Vector(0, container_height - container_thickness, container_size.z - container_thickness), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(Bottom, 100000, Vector(0, container_height - container_size.y, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
InitObject(Top, 100000, Vector(0, container_height + container_size.y, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
ChSharedPtr<ChMaterialSurface> material_tube;
material_tube = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material_tube->SetFriction(0);
material_tube->SetRollingFriction(0);
material_tube->SetSpinningFriction(0);
material_tube->SetCompliance(0);
material_tube->SetCohesion(-100);
InitObject(Tube, 100000, Vector(0, 0, 0), Quaternion(1, 0, 0, 0), material_tube, true, true, -20, -20);
AddCollisionGeometry(L, BOX, Vector(container_thickness, container_size.y, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(R, BOX, Vector(container_thickness, container_size.y, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(F, BOX, Vector(container_size.x, container_size.y, container_thickness), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(B, BOX, Vector(container_size.x, container_size.y, container_thickness), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Bottom, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Top, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
real tube_size = .03;
AddCollisionGeometry(Tube, BOX, Vector(tube_size * 5, container_thickness / 6.0, tube_size), Vector(0, tube_size, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Tube, BOX, Vector(tube_size * 5, container_thickness / 6.0, tube_size), Vector(0, -tube_size, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Tube, BOX, Vector(tube_size * 5, tube_size, container_thickness / 6.0), Vector(0, 0, -tube_size), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Tube, BOX, Vector(tube_size * 5, tube_size, container_thickness / 6.0), Vector(0, 0, tube_size), Quaternion(1, 0, 0, 0));
FinalizeObject(L, (ChSystemParallel *) system_gpu);
FinalizeObject(R, (ChSystemParallel *) system_gpu);
FinalizeObject(F, (ChSystemParallel *) system_gpu);
FinalizeObject(B, (ChSystemParallel *) system_gpu);
FinalizeObject(Bottom, (ChSystemParallel *) system_gpu);
//FinalizeObject(Top, (ChSystemParallel *) system_gpu);
//FinalizeObject(Tube, (ChSystemParallel *) system_gpu);
material_fiber = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material_fiber->SetFriction(.4);
material_fiber->SetRollingFriction(.8);
material_fiber->SetSpinningFriction(.8);
material_fiber->SetCompliance(0);
material_fiber->SetCohesion(0);
layer_gen = new ParticleGenerator<ChSystemParallel>((ChSystemParallel *) system_gpu);
layer_gen->SetDensity(1000);
layer_gen->SetRadius(R3(particle_radius));
layer_gen->material->SetFriction(.1);
layer_gen->material->SetCohesion(.1);
layer_gen->material->SetSpinningFriction(0);
layer_gen->material->SetRollingFriction(0);
//layer_gen->SetCylinderRadius(4.5);
//layer_gen->SetNormalDistribution(particle_radius, .002);
layer_gen->AddMixtureType(MIX_SPHERE);
//layer_gen->addPerturbedVolumeMixture(R3(0,-1,0), I3(80,40,80),R3(.1,.1,.1),R3(0,0,0),0);
//=========================================================================================================
//Rendering specific stuff:
ChOpenGLManager * window_manager = new ChOpenGLManager();
ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
//openGLView.render_camera->camera_position = glm::vec3(0, -5, -10);
//openGLView.render_camera->camera_look_at = glm::vec3(0, -5, 0);
openGLView.render_camera->camera_scale = .1;
openGLView.SetCustomCallback(RunTimeStep);
openGLView.StartSpinning(window_manager);
window_manager->CallGlutMainLoop();
//=========================================================================================================
int file = 0;
for (int i = 0; i < num_steps; i++) {
system_gpu->DoStepDynamics(timestep);
double TIME = system_gpu->GetChTime();
double STEP = system_gpu->GetTimerStep();
double BROD = system_gpu->GetTimerCollisionBroad();
double NARR = system_gpu->GetTimerCollisionNarrow();
double LCP = system_gpu->GetTimerLcp();
double UPDT = system_gpu->GetTimerUpdate();
double RESID = ((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->GetResidual();
int BODS = system_gpu->GetNbodies();
int CNTC = system_gpu->GetNcontacts();
int REQ_ITS = ((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->GetTotalIterations();
printf("%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7d|%7d|%7d|%7.4f\n", TIME, STEP, BROD, NARR, LCP, UPDT, BODS, CNTC, REQ_ITS, RESID);
int save_every = 1.0 / timestep / 60.0; //save data every n steps
if (i % save_every == 0) {
stringstream ss;
cout << "Frame: " << file << endl;
ss << "data/string/" << "/" << file << ".txt";
//DumpAllObjects(system_gpu, ss.str(), ",", true);
DumpAllObjectsWithGeometryPovray(system_gpu, ss.str());
//output.ExportData(ss.str());
file++;
}
RunTimeStep(system_gpu, i);
}
//DumpObjects(system_gpu, "diagonal_impact_settled.txt", "\t");
}
int main(int argc, char* argv[]) {
//omp_set_num_threads(8);
//=========================================================================================================
ChSystemParallelDVI * system_gpu = new ChSystemParallelDVI;
system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);
//=========================================================================================================
//ReadInputFile("convergence_config.txt",system_gpu);
//system_gpu->SetMaxiter(max_iter);
//system_gpu->SetIterLCPmaxItersSpeed(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationNormal(15);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSliding(0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSpinning(0);
system_gpu->SetTol(tolerance);
system_gpu->SetTolSpeeds(tolerance);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(tolerance);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance( 0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(1000);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(APGDRS);
//((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->solver.SetAPGDParams(10, .9, 2.0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->DoStabilization(false);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->DoCollision(false);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetWarmStart(false);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius * .01);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBinsPerAxis(I3(20, 20, 20));
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBodyPerBin(100, 50);
//=========================================================================================================
system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
system_gpu->SetStep(timestep);
ReadAllObjectsWithGeometryChrono(system_gpu, "dump_sor_heavy_.4.txt");
system_gpu->Get_bodylist()->at(5)->SetMass(100000);
/*
//=========================================================================================================
ChSharedBodyPtr L = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr R = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr F = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr B = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr BTM = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
BLOCK = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedPtr<ChMaterialSurface> material;
material = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material->SetFriction(plate_friction);
material->SetCompliance(0);
InitObject(L, 100000, Vector(-container_width + container_thickness, plate_height, 0), quat, material, true, true, -20, -20);
InitObject(R, 100000, Vector(container_width - container_thickness, plate_height, 0), quat, material, true, true, -20, -20);
InitObject(F, 100000, Vector(0, plate_height, -container_width + container_thickness), quat, material, true, true, -20, -20);
InitObject(B, 100000, Vector(0, plate_height, container_width - container_thickness), quat, material, true, true, -20, -20);
InitObject(BTM, 100000, Vector(0, plate_height - container_height, 0), quat, material, true, true, -20, -20);
InitObject(BLOCK, 10000, Vector(0, 5-.5, 0), quat, material, true, false, -1, -20);
AddCollisionGeometry(L, BOX, Vector(container_thickness, container_height, container_width), lpos, quat);
AddCollisionGeometry(R, BOX, Vector(container_thickness, container_height, container_width), lpos, quat);
AddCollisionGeometry(F, BOX, Vector(container_width * wscale, container_height, container_thickness), lpos, quat);
AddCollisionGeometry(B, BOX, Vector(container_width * wscale, container_height, container_thickness), lpos, quat);
AddCollisionGeometry(BTM, BOX, Vector(container_width * wscale, container_thickness, container_width), lpos, quat);
AddCollisionGeometry(BLOCK, BOX, Vector(container_width, container_thickness, container_width), lpos, quat);
FinalizeObject(L, (ChSystemParallel *) system_gpu);
FinalizeObject(R, (ChSystemParallel *) system_gpu);
FinalizeObject(F, (ChSystemParallel *) system_gpu);
FinalizeObject(B, (ChSystemParallel *) system_gpu);
FinalizeObject(BTM, (ChSystemParallel *) system_gpu);
FinalizeObject(BLOCK, (ChSystemParallel *) system_gpu);
BLOCK->SetPos_dt(ChVector<>(0,-4,0));
ParticleGenerator<ChSystemParallel> layer_gen(system_gpu);
layer_gen.SetDensity(1000);
layer_gen.SetRadius(R3(particle_radius));
layer_gen.material->SetFriction(1);
layer_gen.material->SetRollingFriction(0);
layer_gen.material->SetSpinningFriction(0);
layer_gen.material->SetCohesion(0);
layer_gen.material->SetCompliance(1e-5);
int3 num_per_dir = I3(2.0 / particle_radius, 4.0 / particle_radius, 2.0 / particle_radius);
layer_gen.AddMixtureType(MIX_SPHERE);
//layer_gen.AddMixtureType(MIX_ELLIPSOID);
//layer_gen.AddMixtureType(MIX_CONE);
//layer_gen.AddMixtureType(MIX_CUBE);
//layer_gen.AddMixtureType(MIX_CYLINDER);
//layer_gen.SetNormalDistribution(rad.x, rad.x/4.0);
//layer_gen->UseNormalCohesion(particle_cohesion, 1);
layer_gen.addPerturbedVolumeMixture(R3(0, -.8, 0), num_per_dir, R3(0.1, .1, 0.1), R3(0, -4, 0));
*/
//=========================================================================================================
//////Rendering specific stuff:
ChOpenGLManager * window_manager = new ChOpenGLManager();
ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
//openGLView.render_camera->camera_position = glm::vec3(0, -5, -40);
//openGLView.render_camera->camera_look_at = glm::vec3(0, -5, 0);
//openGLView.render_camera->camera_scale = .1;
openGLView.SetCustomCallback(RunTimeStep);
openGLView.StartSpinning(window_manager);
window_manager->CallGlutMainLoop();
//=========================================================================================================
//timestep
//iterations
//warm start
//mass
ofstream ofile("convergence.txt");
ChTimer<double> timer;
timer.start();
int file = 0;
for (int i = 0; i < num_steps; i++) {
cout << "step " << i << endl;
system_gpu->DoStepDynamics(timestep);
RunTimeStep(system_gpu, i);
//if (i % save_every == 0) {
ofile << ((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->GetResidual() << endl;
// stringstream ss;
// cout << "Frame: " << file << endl;
// DumpObjects(system_gpu, ss.str());
// //output.ExportData(ss.str());
// file++;
//}
timer.stop();
}
//DumpAllObjectsWithGeometryChrono(system_gpu, "dump.txt");
cout << "TIME: " << timer() << endl;
ofile.close();
return 0;
}
void RunTimeStep(T* mSys, const int frame) {
if (!save) {
Vector force = engine_anchor->Get_react_force();
Vector torque = engine_anchor->Get_react_torque();
double motor_torque = engine_anchor->Get_mot_torque();
cout << force.x << " " << force.y << " " << force.z << " " << torque.x << " " << torque.y << " " << torque.z << " " << motor_torque << " " << ANCHOR->GetPos().y << " "
<< ANCHOR->GetPos_dt().y << endl;
ANCHOR->SetPos(ChVector<>(REFERENCE->GetPos().x, ANCHOR->GetPos().y, REFERENCE->GetPos().z));
ANCHOR->SetPos_dt(ChVector<>(REFERENCE->GetPos_dt().x, ANCHOR->GetPos_dt().y, REFERENCE->GetPos_dt().z));
ANCHOR->SetRot(REFERENCE->GetRot());
ANCHOR->SetWvel_loc(REFERENCE->GetWvel_loc());
REFERENCE->SetPos(ChVector<>(REFERENCE->GetPos().x, ANCHOR->GetPos().y, REFERENCE->GetPos().z));
REFERENCE->SetPos_dt(ChVector<>(REFERENCE->GetPos_dt().x, ANCHOR->GetPos_dt().y, REFERENCE->GetPos_dt().z));
}
// real t = frame * timestep * PI * 2 * frequency;
//
// BLOCK->SetRot(ChQuaternion<>(1, 0, 0, 0));
// BLOCK->SetWvel_loc(ChVector<>(0, 0, 0));
// BLOCK->SetPos(ChVector<>(sin(t) * amplitude, BLOCK->GetPos().y, 0));
// BLOCK->SetPos_dt(ChVector<>(cos(t) * amplitude * 2 * PI * frequency, BLOCK->GetPos_dt().y, 0));
//
// CONTAINER->SetPos(ChVector<>(sin(t) * amplitude, 0, 0));
// CONTAINER->SetPos_dt(ChVector<>(cos(t) * amplitude * 2 * PI * frequency, 0, 0));
// CONTAINER->SetWvel_loc(ChVector<>(0, 0, 0));
// CONTAINER->SetRot(ChQuaternion<>(1, 0, 0, 0));
//
//
// real cont_vol = (container_size.x - container_thickness * 2) * 2 * (BLOCK->GetPos().y + container_size.y - 2 * container_thickness) * (container_size.z - container_thickness * 2) * 2;
// cout << layer_gen->total_volume << " " << layer_gen->total_mass << " " << cont_vol << " " << layer_gen->total_mass / cont_vol << endl;
//
if (save) {
if (layers < 130 && frame % 80 == 0) {
cout << layers << endl;
layer_gen->addPerturbedVolumeMixture(R3(0, -container_size.y + container_thickness + particle_radius * 5 + frame / 14.0, 0), I3(32, 1, 32), R3(0, 0, 0), R3(0, 0, 0));
layers++;
}
} else {
//300 - 457.2
// double time = actuator_anchor->GetChTime();
// if (ANCHOR->GetPos().y <= 300 - 457.2 && once) {
// motionFunc1->Set_y0(time * -anchor_vel);
// motionFunc1->Set_ang(-2);
//// motionFunc2->Set_y0(time * -anchor_rot * 1 / 60.0 * 2 * CH_C_PI);
//// motionFunc2->Set_ang(0);
// if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(engine_anchor->Get_spe_funct())) {
// mfun->Set_yconst(0); // rad/s angular speed
// }
// once = false;
// }
}
}
int main(int argc, char* argv[]) {
save = atoi(argv[1]);
cout << save << endl;
if (save) {
seconds_to_simulate = 5;
num_steps = seconds_to_simulate / timestep;
} else {
seconds_to_simulate = 300;
num_steps = seconds_to_simulate / timestep;
}
if (argc > 2) {
particle_slide_friction = atof(argv[2]);
particle_roll_friction = atof(argv[3]);
particle_cohesion = atof(argv[4]);
data_folder = argv[5];
cout << particle_slide_friction << " " << particle_roll_friction << " " << particle_roll_friction << endl;
}
//=========================================================================================================
ChSystemParallelDVI * system_gpu = new ChSystemParallelDVI;
//=========================================================================================================
//system_gpu->SetMinThreads(32);
//system_gpu->SetMaxiter(max_iter);
//system_gpu->SetIterLCPmaxItersSpeed(max_iter);
((ChSystemParallelDVI*) system_gpu)->DoThreadTuning(true);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationNormal(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSliding(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationSpinning(max_iter);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIterationBilateral(0);
system_gpu->SetTol(tolerance);
system_gpu->SetTolSpeeds(tolerance);
system_gpu->SetMaxPenetrationRecoverySpeed(1e9);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(tolerance);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance(0);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(100);
((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(APGDRS);
//((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->DoStabilization(true);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius * .05);
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBinsPerAxis(I3(30, 60, 30));
((ChCollisionSystemParallel *) (system_gpu->GetCollisionSystem()))->setBodyPerBin(100, 50);
system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
system_gpu->SetStep(timestep);
((ChSystemParallel*) system_gpu)->SetAABB(R3(-250, -600, -250), R3(250, 600, 250));
//=========================================================================================================
ChSharedPtr<ChMaterialSurface> material;
material = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material->SetFriction(container_friction);
material->SetRollingFriction(container_friction);
material->SetSpinningFriction(container_friction);
material->SetCompliance(0);
material->SetCohesion(-100);
CONTAINER = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(CONTAINER, 100000, Vector(0, 0, 0), Quaternion(1, 0, 0, 0), material, true, true, -20, -20);
if (save) {
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_thickness, container_size.y, container_size.z),
Vector(-container_size.x + container_thickness, container_height - container_thickness, 0),
Quaternion(1, 0, 0, 0));
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_thickness, container_size.y, container_size.z),
Vector(container_size.x - container_thickness, container_height - container_thickness, 0),
Quaternion(1, 0, 0, 0));
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_size.x, container_size.y, container_thickness),
Vector(0, container_height - container_thickness, -container_size.z + container_thickness),
Quaternion(1, 0, 0, 0));
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_size.x, container_size.y, container_thickness),
Vector(0, container_height - container_thickness, container_size.z - container_thickness),
Quaternion(1, 0, 0, 0));
AddCollisionGeometry(
CONTAINER,
BOX,
Vector(container_size.x, container_thickness, container_size.z),
Vector(0, container_height - container_size.y, 0),
Quaternion(1, 0, 0, 0));
//AddCollisionGeometry(CONTAINER, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, container_height + container_size.y, 0), Quaternion(1, 0, 0, 0));
}
CONTAINER->GetMaterialSurface()->SetCohesion(container_cohesion);
FinalizeObject(CONTAINER, (ChSystemParallel *) system_gpu);
if (save == false) {
real anchor_length = 100;
real anchor_r = 35 / 2.0;
real anchor_R = 150 / 4.0;
real anchor_h = 50;
real anchor_thickness = 6 / 2.0;
real anchor_blade_width = 8;
ChVector<> p1(0, 0, 0);
ChVector<> p2(0, anchor_length, 0);
real anchor_mass = 6208;
real number_sections = 30;
REFERENCE = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(REFERENCE, anchor_mass, Vector(0, 200, 0), Quaternion(1, 0, 0, 0), material, false, false, -15, -15);
// AddCollisionGeometry(REFERENCE, SPHERE, ChVector<>(anchor_r, 0, 0), p1, Quaternion(1, 0, 0, 0));
// AddCollisionGeometry(REFERENCE, CYLINDER, Vector(anchor_r, anchor_length, anchor_r), p2, Quaternion(1, 0, 0, 0));
// for (int i = 0; i < number_sections; i++) {
// ChQuaternion<> quat, quat2;
// quat.Q_from_AngAxis(i / number_sections * 2 * PI, ChVector<>(0, 1, 0));
// quat2.Q_from_AngAxis(6.5 * 2 * PI / 360.0, ChVector<>(0, 0, 1));
// quat = quat % quat2;
// ChVector<> pos(sin(i / number_sections * 2 * PI) * anchor_R, i / number_sections * anchor_h, cos(i / number_sections * 2 * PI) * anchor_R);
// //ChMatrix33<> mat(quat);
// AddCollisionGeometry(REFERENCE, BOX, ChVector<>(anchor_blade_width, anchor_thickness, anchor_R), pos, quat);
// }
ANCHOR = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(ANCHOR, anchor_mass, Vector(0, 200, 0), Quaternion(1, 0, 0, 0), material, true, false, -15, -15);
AddCollisionGeometry(ANCHOR, SPHERE, ChVector<>(anchor_r, 0, 0), p1, Quaternion(1, 0, 0, 0));
AddCollisionGeometry(ANCHOR, CYLINDER, Vector(anchor_r, anchor_length, anchor_r), p2, Quaternion(1, 0, 0, 0));
//
for (int i = 0; i < number_sections; i++) {
ChQuaternion<> quat, quat2;
quat.Q_from_AngAxis(i / number_sections * 2 * PI, ChVector<>(0, 1, 0));
quat2.Q_from_AngAxis(6.5 * 2 * PI / 360.0, ChVector<>(0, 0, 1));
quat = quat % quat2;
ChVector<> pos(sin(i / number_sections * 2 * PI) * anchor_R, i / number_sections * anchor_h, cos(i / number_sections * 2 * PI) * anchor_R);
//ChMatrix33<> mat(quat);
AddCollisionGeometry(ANCHOR, BOX, ChVector<>(anchor_blade_width, anchor_thickness, anchor_R), pos, quat);
}
FinalizeObject(ANCHOR, (ChSystemParallel *) system_gpu);
FinalizeObject(REFERENCE, (ChSystemParallel *) system_gpu);
// real vol = ((ChCollisionModelParallel *) ANCHOR->GetCollisionModel())->getVolume();
// real den = .00785;
// anchor_mass = den*vol;
// cout<<vol<<" "<<anchor_mass<<endl;
// ANCHOR->SetMass(anchor_mass);
ANCHOR->SetInertiaXX(ChVector<>(12668786.72, 5637598.31, 12682519.69));
REFERENCE->SetInertiaXX(ChVector<>(12668786.72, 5637598.31, 12682519.69));
// ANCHOR->SetInertiaXX(
// ChVector<>(
// 1 / 12.0 * anchor_mass * (1 * 1 + anchor_R * anchor_R),
// 1 / 12.0 * anchor_mass * (anchor_R * anchor_R + anchor_R * anchor_R),
// 1 / 12.0 * anchor_mass * (anchor_R * anchor_R + 1 * 1)));
//BLOCK = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
//InitObject(BLOCK, anchor_mass/2, Vector(0, 300, 0), Quaternion(1, 0, 0, 0), material, false, false, -20, -20);
//AddCollisionGeometry(BLOCK, BOX, ChVector<>(1, 1, 1), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
//FinalizeObject(BLOCK, (ChSystemParallel *) system_gpu);
//actuator_anchor = ChSharedPtr<ChLinkLockLock>(new ChLinkLockLock());
//actuator_anchor->Initialize(CONTAINER, BLOCK, ChCoordsys<>(ChVector<>(0, 0, 0), QUNIT));
//system_gpu->AddLink(actuator_anchor);
// apply motion
//motionFunc1 = new ChFunction_Ramp(0, -anchor_vel);
//actuator_anchor->SetMotion_Y(motionFunc1);
//actuator_anchor->SetMotion_axis(ChVector<>(0, 1, 0));
engine_anchor = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
engine_anchor->Initialize(CONTAINER, ANCHOR, ChCoordsys<>(ANCHOR->GetPos(), chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_X)));
engine_anchor->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_PRISM); // also works as revolute support
engine_anchor->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
system_gpu->AddLink(engine_anchor);
reference_engine = ChSharedPtr<ChLinkEngine>(new ChLinkEngine);
reference_engine->Initialize(CONTAINER, REFERENCE, ChCoordsys<>(REFERENCE->GetPos(), chrono::Q_from_AngAxis(CH_C_PI / 2, VECT_X)));
reference_engine->Set_shaft_mode(ChLinkEngine::ENG_SHAFT_PRISM); // also works as revolute support
reference_engine->Set_eng_mode(ChLinkEngine::ENG_MODE_SPEED);
system_gpu->AddLink(reference_engine);
if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(engine_anchor->Get_spe_funct())) {
mfun->Set_yconst(anchor_rot * 1 / 60.0 * 2 * CH_C_PI); // rad/s angular speed
}
if (ChFunction_Const* mfun = dynamic_cast<ChFunction_Const*>(reference_engine->Get_spe_funct())) {
mfun->Set_yconst(anchor_rot * 1 / 60.0 * 2 * CH_C_PI); // rad/s angular speed
}
ReadAllObjectsWithGeometryChrono(system_gpu, "data/anchor/anchor.dat");
ChSharedPtr<ChMaterialSurface> material_read;
material_read = ChSharedPtr<ChMaterialSurface>(new ChMaterialSurface);
material_read->SetFriction(particle_slide_friction);
material_read->SetRollingFriction(particle_roll_friction);
material_read->SetSpinningFriction(particle_roll_friction);
material_read->SetCompliance(0);
material_read->SetCohesion(particle_cohesion * timestep);
for (int i = 0; i < system_gpu->Get_bodylist()->size(); i++) {
system_gpu->Get_bodylist()->at(i)->SetMaterialSurface(material_read);
}
} else {
layer_gen = new ParticleGenerator<ChSystemParallelDVI>((ChSystemParallelDVI *) system_gpu);
layer_gen->SetDensity(particle_density);
layer_gen->SetRadius(R3(particle_radius, particle_radius * .5, particle_radius));
layer_gen->SetNormalDistribution(particle_radius, particle_std_dev, 1);
layer_gen->material->SetFriction(particle_slide_friction);
layer_gen->material->SetCohesion(particle_cohesion);
layer_gen->material->SetRollingFriction(0);
layer_gen->material->SetSpinningFriction(0);
layer_gen->AddMixtureType(MIX_SPHERE);
}
//layer_gen->AddMixtureType(MIX_ELLIPSOID);
//layer_gen->AddMixtureType(MIX_DOUBLESPHERE);
//layer_gen->addPerturbedVolumeMixture(R3(0, 0, 0), I3(64, 1, 64), R3(0, 0, 0), R3(0, 0, 0));
//=========================================================================================================
//Rendering specific stuff:
// ChOpenGLManager * window_manager = new ChOpenGLManager();
// ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
// openGLView.render_camera->camera_position = glm::vec3(0, -5, -10);
// openGLView.render_camera->camera_look_at = glm::vec3(0, 0, 0);
// openGLView.render_camera->camera_scale = 2;
// openGLView.SetCustomCallback(RunTimeStep);
// openGLView.StartSpinning(window_manager);
// window_manager->CallGlutMainLoop();
//=========================================================================================================
int file = 0;
ofstream reactionfile;
if (save == false) {
stringstream ss;
ss << data_folder << "reactions.txt";
reactionfile.open(ss.str());
}
for (int i = 0; i < num_steps; i++) {
system_gpu->DoStepDynamics(timestep);
double TIME = system_gpu->GetChTime();
double STEP = system_gpu->GetTimerStep();
double BROD = system_gpu->GetTimerCollisionBroad();
double NARR = system_gpu->GetTimerCollisionNarrow();
double LCP = system_gpu->GetTimerLcp();
double SHUR = ((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->solver.time_shurcompliment;
double UPDT = system_gpu->GetTimerUpdate();
double RESID = ((ChLcpSolverParallelDVI *) (system_gpu->GetLcpSolverSpeed()))->GetResidual();
int BODS = system_gpu->GetNbodies();
int CNTC = system_gpu->GetNcontacts();
int REQ_ITS = ((ChLcpSolverParallelDVI*) (system_gpu->GetLcpSolverSpeed()))->GetTotalIterations();
printf("%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7d|%7d|%7d|%7.4f\n", TIME, STEP, BROD, NARR, LCP, SHUR, UPDT, BODS, CNTC, REQ_ITS, RESID);
system_gpu->gpu_data_manager->system_timer.PrintReport();
int save_every = 1.0 / timestep / 60.0; //save data every n steps
if (i % save_every == 0) {
stringstream ss;
cout << "Frame: " << file << endl;
ss << data_folder << "/" << file << ".txt";
if (save) {
DumpAllObjectsWithGeometryChrono(system_gpu, "data/anchor/anchor.dat");
} else {
DumpAllObjectsWithGeometryPovray(system_gpu, ss.str());
}
file++;
}
if (save == false) {
Vector force = engine_anchor->Get_react_force();
Vector torque = engine_anchor->Get_react_torque();
double motor_torque = engine_anchor->Get_mot_torque();
reactionfile << force.x << " " << force.y << " " << force.z << " " << torque.x << " " << torque.y << " " << torque.z << " " << motor_torque << " " << ANCHOR->GetPos().y
<< " " << ANCHOR->GetPos_dt().y << endl;
}
RunTimeStep(system_gpu, i);
}
if (save == false) {
reactionfile.close();
}
}
#include "../../common/input_output.h"
real gravity = -9.80665;
real timestep = .0005;
real3 particle_radius = R3(.1, .05, .1);
real seconds_to_simulate = 5.0;
int max_iter = 25;
int num_steps = seconds_to_simulate / timestep;
real3 container_size = R3(10, 2, 30);
real container_thickness = .25;
real container_height = 4;
real container_friction = .5;
int3 num_per_dir = I3((container_size - particle_radius * 2) / particle_radius);
template<class T>
void RunTimeStep(T* mSys, const int frame) {
}
int main(int argc, char* argv[]) {
if (argc == 2) {
omp_set_num_threads(atoi(argv[1]));
} else {
omp_set_num_threads(1);
}
F3(A, B4G4R4X4_UNORM, NONE, B, G, R, xx, UNORM, A4B4G4R4, T), F3(A, R9G9B9E5_FLOAT, NONE, R, G, B, xx, FLOAT, E5B9G9R9_SHAREDEXP, T), C4(A, R10G10B10A2_UNORM, RGB10_A2_UNORM, R, G, B, A, UNORM, A2B10G10R10, IB), C4(A, B10G10R10A2_UNORM, BGR10_A2_UNORM, B, G, R, A, UNORM, A2B10G10R10, TD), C4(A, R10G10B10A2_SNORM, NONE, R, G, B, A, SNORM, A2B10G10R10, T), C4(A, B10G10R10A2_SNORM, NONE, B, G, R, A, SNORM, A2B10G10R10, T), C4(A, R10G10B10A2_UINT, RGB10_A2_UINT, R, G, B, A, UINT, A2B10G10R10, TR), C4(A, B10G10R10A2_UINT, RGB10_A2_UINT, B, G, R, A, UINT, A2B10G10R10, T), F3(A, R11G11B10_FLOAT, R11G11B10_FLOAT, R, G, B, xx, FLOAT, BF10GF11RF11, IB), F3(A, L8_UNORM, R8_UNORM, R, R, R, xx, UNORM, R8, TB), F3(A, L8_SRGB, NONE, R, R, R, xx, UNORM, R8, T), F3(A, L8_SNORM, R8_SNORM, R, R, R, xx, SNORM, R8, TC), I3(A, L8_SINT, R8_SINT, R, R, R, xx, SINT, R8, TR), I3(A, L8_UINT, R8_UINT, R, R, R, xx, UINT, R8, TR), F3(A, L16_UNORM, R16_UNORM, R, R, R, xx, UNORM, R16, TC), F3(A, L16_SNORM, R16_SNORM, R, R, R, xx, SNORM, R16, TC), F3(A, L16_FLOAT, R16_FLOAT, R, R, R, xx, FLOAT, R16, TB), I3(A, L16_SINT, R16_SINT, R, R, R, xx, SINT, R16, TR), I3(A, L16_UINT, R16_UINT, R, R, R, xx, UINT, R16, TR), F3(A, L32_FLOAT, R32_FLOAT, R, R, R, xx, FLOAT, R32, TB), I3(A, L32_SINT, R32_SINT, R, R, R, xx, SINT, R32, TR), I3(A, L32_UINT, R32_UINT, R, R, R, xx, UINT, R32, TR), C4(A, I8_UNORM, R8_UNORM, R, R, R, R, UNORM, R8, TR), C4(A, I8_SNORM, R8_SNORM, R, R, R, R, SNORM, R8, TR), C4(A, I8_SINT, R8_SINT, R, R, R, R, SINT, R8, TR), C4(A, I8_UINT, R8_UINT, R, R, R, R, UINT, R8, TR), C4(A, I16_UNORM, R16_UNORM, R, R, R, R, UNORM, R16, TR),
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
//bridge that will transform the message (image) from ROS code back to "image" code
sensor_msgs::CvBridge bridge;
fprintf(stderr, "\n call Back funtion \n");
//publish data (obstacle waypoints) back to the boat
//ros::NodeHandle n;
//std_msgs::Float32 xWaypoint_msg; // X coordinate obstacle message
//std_msgs::Float32 yWaypoint_msg; // Y coordinate obstacle message
//publish the waypoint data
//ros::Publisher waypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//ros::Publisher Ywaypoint_info_pub = n.advertise<std_msgs::Float32>("waypoint_info", 1000);
//std::stringstream ss;
/***********************************************************************/
//live image coming streamed straight from the boat's camera
IplImage* boatFront = bridge.imgMsgToCv(msg, "bgr8");
//The boat takes flipped images, so you need to flip them back to normal
cvFlip(boatFront, boatFront, 0);
IplImage* backUpImage = cvCloneImage(boatFront);
boatFront->origin = IPL_ORIGIN_TL; //sets image origin to top left corner
int X = boatFront->height;
int Y = boatFront->width;
//cout << "height " << X << endl;
//cout << "width " << Y << endl;
/*********************Image Filtering variables****************************/
//these images are used for segmenting objects from the overall background
//create a one channel image to convert from RGB to GRAY
IplImage* grayImage = cvCreateImage(cvGetSize(boatFront),IPL_DEPTH_8U,1);
//convert grayImage to binary (final step after converting from GRAY)
IplImage* bwImage = cvCreateImage(cvGetSize(grayImage),IPL_DEPTH_8U,1);
//variables used for the flood fill segmentation
CvPoint seed_point = cvPoint(boatFront->height/1.45,0); //not sure how this variable works
CvScalar color = CV_RGB(250,0,0);
CvMemStorage* grayStorage = NULL; //memory storage for contour sequence
CvSeq* contours = 0;
// get blobs and filter them using their area
//IplConvKernel* morphKernel = cvCreateStructuringElementEx(5, 5, 1, 1, CV_SHAPE_RECT, NULL);
//IplImage* original, *originalThr;
//IplImage* segmentated = cvCreateImage(cvGetSize(boatFront), 8, 1);
//unsigned int blobNumber = 0;
//IplImage* labelImg = cvCreateImage(cvGetSize(boatFront), IPL_DEPTH_LABEL, 1);
CvMoments moment;
/***********************************************************************/
//boat's edge distance from the camera. This is used for visual calibration
//to know the distance from the boat to the nearest obstacles.
//With respect to the mounted camera, distance is 21 inches (0.5334 m) side to side
//and 15 inches (0.381 m).
//float boatFrontDistance = 0.381; //distance in meters
//float boatSideDistance = 0.5334; //distance in meters
// These variables tell the distance from the center bottom of the image
// (the camera) to the square surrounding a the obstacle
float xObstacleDistance = 0.0;
float yObstacleDistance = 0.0;
float obstacleDistance = 0.0;
float obstacleHeading = 0.0;
int pixelsNumber = 50; //number of pixels for an n x n matrix and # of neighbors
const int arraySize = pixelsNumber;
const int threeArraySize = pixelsNumber;
//if n gets changed, then the algorithm might have to be
//recalibrated. Try to keep it constant
//these variables are used for the k nearest neighbors
//int accuracy;
//reponses for each of the classifications
float responseWaterH, responseWaterS, responseWaterV;
float responseGroundH, responseGroundS, responseGroundV;
float responseSkyH, responseSkyS, responseSkyV;
float averageHue = 0.0;
float averageSat = 0.0;
float averageVal = 0.0;
CvMat* trainClasses = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClasses2 = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
//CvMat sample = cvMat( 1, 2, CV_32FC1, _sample );
//used with the classifier
CvMat* trainClassesH = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClassesS = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
CvMat* trainClassesV = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
//CvMat* trainClasses2 = cvCreateMat( pixelsNumber, 1, CV_32FC1 );
//CvMat sample = cvMat( 1, 2, CV_32FC1, _sample );
//used with the classifier
/*CvMat* nearestWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* nearestSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1);
//Distance
CvMat* distanceWaterH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceWaterS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceWaterV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceGroundH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceGroundS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceGroundV = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceSkyH = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceSkyS = cvCreateMat(1, pixelsNumber, CV_32FC1);
CvMat* distanceSkyV = cvCreateMat(1, pixelsNumber, CV_32FC1); */
//these variables are use to traverse the picture by blocks of n x n pixels at
//a time.
//Index(0,0) does not exist, so make sure kj and ki start from 1 (in the
//right way, of course)
//x and y are the dimensions of the local patch of pixels
int x = (boatFront->height)/1.45;//(boatFront->height)/2.5 + 105;
int y = 0;
int skyX = 0;
int skyY = 0;
int row1 = 0;
int column1 = 0;
//these two variables are used in order to divide the grid in the
//resample segmentation part
int xDivisor = 200;
int yDivisor = 200;
//ground sample
//CvMat* groundTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//CvMat* groundTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//CvMat* groundTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//water sample
CvMat* waterTrainingHue = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingSat = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
CvMat* waterTrainingVal = cvCreateMat(threeArraySize,arraySize,CV_32FC1);
//n x n sample patch taken from the picture
CvMat* sampleHue = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleSat = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* sampleVal = cvCreateMat(1,arraySize,CV_32FC1);
CvMat* resampleHue = cvCreateMat(boatFront->height/xDivisor,boatFront->width/yDivisor,CV_32FC1);
CvMat* resampleSat = cvCreateMat(boatFront->height/xDivisor,boatFront->width/yDivisor,CV_32FC1);
CvMat* resampleVal = cvCreateMat(boatFront->height/xDivisor,boatFront->width/yDivisor,CV_32FC1);
int xDiv = 20;
int yDiv = 20;
CvMat* resampleHue2 = cvCreateMat(boatFront->height/xDiv,boatFront->width/yDiv,CV_32FC1);
CvMat* resampleSat2 = cvCreateMat(boatFront->height/xDiv,boatFront->width/yDiv,CV_32FC1);
CvMat* resampleVal2 = cvCreateMat(boatFront->height/xDiv,boatFront->width/yDiv,CV_32FC1);
//sky training sample
CvMat* skyTrainingHue = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingSat = cvCreateMat(arraySize,arraySize,CV_32FC1);
CvMat* skyTrainingVal = cvCreateMat(arraySize,arraySize,CV_32FC1);
//initialize each matrix element to zero for ease of use
//cvZero(groundTrainingHue);
//cvZero(groundTrainingSat);
//cvZero(groundTrainingVal);
cvZero(waterTrainingHue);
cvZero(waterTrainingSat);
cvZero(waterTrainingVal);
cvZero(sampleHue);
cvZero(sampleSat);
cvZero(sampleVal);
cvZero(resampleHue);
cvZero(resampleSat);
cvZero(resampleVal);
cvZero(skyTrainingHue);
cvZero(skyTrainingSat);
cvZero(skyTrainingVal);
//Stores the votes for each channel (whether it belongs to water or not
//1 is part of water, 0 not part of water
//if sum of votes is bigger than 1/2 the number of elements, then it belongs to water
int votesSum = 0;
int comparator[3]; //used when only three votes are needed
//int comparatorTwo [3][3]; //used when six votes are needed
//initial sum of votes is zero
//Error if initialize both matrices inside a single for loop. Dont know why
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
/***********************************************************************/
//Convert from RGB to HSV to control the brightness of the objects.
//work with reflexion
/*Sky recognition. Might be useful for detecting reflexion on the water. If
the sky is detected, and the reflection has the same characteristics of
something below the horizon, that "something" might be water. Assume sky
wont go below the horizon
*/
//convert from RGB to HSV
cvCvtColor(boatFront, boatFront, CV_BGR2HSV);
cvCvtColor(backUpImage, backUpImage, CV_BGR2HSV);
HsvImage I(boatFront);
HsvImage IBackUp(backUpImage);
//Sky detection
/*
for (int i=0; i<boatFront->height;i++)
{
for (int j=0; j<boatFront->width;j++)
{
//if something is bright enough, consider it sky and store the
//value. HSV values go from 0 to 180 ... RGB goes from 0 to 255
if (((I[i][j].v >= 180) && (I[i][j].s <= 16)))
// && ((I[i][j].h >=10)))) //&& (I[i][j].h <= 144))))
{
//The HSV values vary between 0 and 1
cvmSet(skyTrainingHue,skyX,skyY,I[i][j].h);
cvmSet(skyTrainingSat,skyX,skyY,I[i][j].s);
cvmSet(skyTrainingVal,skyX,skyY,I[i][j].v);
//I[i][j].h = 0.3*180; //H (color)
//I[i][j].s = 0.3*180; //S (color intensity)
//I[i][j].v = 0.6*180; //V (brightness)
if (skyY == pixelsNumber-1)
{
if (skyX == pixelsNumber-1)
skyX = 1;
else
skyX = skyX + 1;
skyY = 1;
}
else
skyY = skyY + 1;
}
}
}
/***********************************************************************/
//offline input pictures. Samples of water properties are taken from these
//pictures to get a range of values for H, S, V that will be stored into a
//pre-defined classifier
IplImage* imageSample1 = cvLoadImage("20110805_032255.jpg");
cvSetImageROI(imageSample1, cvRect(0,0,imageSample1->height/0.5,imageSample1->width/1.83));
cvCvtColor(imageSample1, imageSample1, CV_BGR2HSV);
HsvImage I1(imageSample1);
IplImage* imageSample2 = cvLoadImage("20110805_032257.jpg");
cvCvtColor(imageSample2, imageSample2, CV_BGR2HSV);
HsvImage I2(imageSample2);
IplImage* imageSample3 = cvLoadImage("20110805_032259.jpg");
cvCvtColor(imageSample3, imageSample3, CV_BGR2HSV);
HsvImage I3(imageSample3);
IplImage* imageSample4 = cvLoadImage("20110805_032301.jpg");
cvCvtColor(imageSample4, imageSample4, CV_BGR2HSV);
HsvImage I4(imageSample4);
IplImage* imageSample5 = cvLoadImage("20110805_032303.jpg");
cvCvtColor(imageSample5, imageSample5, CV_BGR2HSV);
HsvImage I5(imageSample5);
IplImage* imageSample6 = cvLoadImage("20110805_032953.jpg");
cvCvtColor(imageSample6, imageSample6, CV_BGR2HSV);
HsvImage I6(imageSample6);
IplImage* imageSample7 = cvLoadImage("20110805_032955.jpg");
cvCvtColor(imageSample7, imageSample7, CV_BGR2HSV);
HsvImage I7(imageSample7);
IplImage* imageSample8 = cvLoadImage("20110805_032957.jpg");
cvCvtColor(imageSample8, imageSample8, CV_BGR2HSV);
HsvImage I8(imageSample8);
IplImage* imageSample9 = cvLoadImage("20110805_032959.jpg");
cvCvtColor(imageSample9, imageSample9, CV_BGR2HSV);
HsvImage I9(imageSample9);
IplImage* imageSample10 = cvLoadImage("20110805_033001.jpg");
cvCvtColor(imageSample10, imageSample10, CV_BGR2HSV);
HsvImage I10(imageSample10);
IplImage* imageSample11 = cvLoadImage("20110805_033009.jpg");
cvCvtColor(imageSample11, imageSample11, CV_BGR2HSV);
HsvImage I11(imageSample11);
IplImage* imageSample12 = cvLoadImage("20110805_033011.jpg");
cvCvtColor(imageSample12, imageSample12, CV_BGR2HSV);
HsvImage I12(imageSample12);
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
row1 = ceil(X/1.2866)+ceil(X/5.237)+i+ceil(-X/3.534545455) + ceil(X/4.8);
column1 = ceil(Y/7.0755)+ceil(Y/21.01622)+j+ceil(X/1.495384615);
averageHue = (I1[row1][column1].h + I2[row1][column1].h + I3[row1][column1].h + I4[row1][column1].h + I5[row1][column1].h +
I6[row1][column1].h + I7[row1][column1].h + I8[row1][column1].h + I9[row1][column1].h + I10[row1][column1].h + I11[row1][column1].h + I12[row1][column1].h) / 12;
averageSat = (I1[row1][column1].s + I2[row1][column1].s + I3[row1][column1].s + I4[row1][column1].s + I5[row1][column1].s +
I6[row1][column1].s + I7[row1][column1].s + I8[row1][column1].s + I9[row1][column1].s + I10[row1][column1].s + I11[row1][column1].s + I12[row1][column1].s) / 12;
averageVal = (I1[row1][column1].v + I2[row1][column1].v + I3[row1][column1].v + I4[row1][column1].v + I5[row1][column1].v +
I6[row1][column1].v + I7[row1][column1].v + I8[row1][column1].v + I9[row1][column1].v + I10[row1][column1].v + I11[row1][column1].v + I12[row1][column1].v) / 12;
//water patch sample (n X n matrix)
cvmSet(waterTrainingHue,i,j,averageHue);
cvmSet(waterTrainingSat,i,j,averageSat);
cvmSet(waterTrainingVal,i,j,averageVal);
//patch is red (this is for me to know where the ground patch sample is)
//I[row1][column1].h = 0;
//I[row1][column1].s = 255;
//I[row1][column1].v = 255;
}
}
//creating a training sample from the an image taken on the fly
row1 = 0;
column1 = 0;
for (int i=0; i<pixelsNumber; i++)
{
for (int j=0; j<pixelsNumber; j++)
{
row1 = ceil(X/1.2866)+ceil(X/5.237)+i+ceil(-X/3.534545455) + ceil(X/4.8);
column1 = ceil(Y/7.0755)+ceil(Y/21.01622)+j+ceil(X/1.495384615);
cvmSet(trainClassesH,i,0,I[row1][column1].h);
cvmSet(trainClassesS,i,0,I[row1][column1].s);
cvmSet(trainClassesV,i,0,I[row1][column1].v);
}
}
//order the water samples in ascending order on order to know a range
cvSort(waterTrainingHue, waterTrainingHue, CV_SORT_ASCENDING);
cvSort(waterTrainingSat, waterTrainingSat, CV_SORT_ASCENDING);
cvSort(waterTrainingVal, waterTrainingVal, CV_SORT_ASCENDING);
// find the maximum and minimum values in the array to create a range
int maxH = cvmGet(waterTrainingHue,0,0);
int maxS = cvmGet(waterTrainingSat,0,0);
int maxV = cvmGet(waterTrainingVal,0,0);
int minH = cvmGet(waterTrainingHue,0,0);
int minS = cvmGet(waterTrainingSat,0,0);
int minV = cvmGet(waterTrainingVal,0,0);
for (int i=0; i < threeArraySize; i++)
{
for (int j=0; j < arraySize; j++)
{
if (cvmGet(waterTrainingHue,i,j) > maxH)
maxH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) > maxS)
maxS = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingVal,i,j) > maxV)
maxV = cvmGet(waterTrainingVal,i,j);
if (cvmGet(waterTrainingHue,i,j) < minH)
minH = cvmGet(waterTrainingHue,i,j);
if (cvmGet(waterTrainingSat,i,j) < minS)
minS = cvmGet(waterTrainingSat,i,j);
if (cvmGet(waterTrainingVal,i,j) < minV)
minV = cvmGet(waterTrainingVal,i,j);
}
}
//cout << "Min Value in the range: " << endl;
//cout << minH << endl;
//cout << minS << endl;
//cout << minV << endl;
//cout << "Max Value in the range: " << endl;
//cout << maxH << endl;
//cout << maxS << endl;
//cout << maxV << endl << endl;
/*********** Main loop. It traverses through the picture**********/
/**********************************************************************/
//Ignore unused parts of the image and convert them to black
// for (int i=0; i<boatFront->height/1.45 - 1;i++)
//{
// for (int j=0; j<Y-1;j++)
// {
// I[i][j].h = 0;
// I[i][j].s = 0;
// I[i][j].v = 0;
// }
// }
/*********************************************************************
// Use nearest neighbors to increase accuracy
skyX = 0;
skyY = 0;
while (x < X-1)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<6;i++)
{
column1 = y+i;
if (column1 > Y-1)
column1 = Y-1;
cvmSet(sampleHue,0,i,I[x][column1].h);
cvmSet(sampleSat,0,i,I[x][column1].s);
cvmSet(sampleVal,0,i,I[x][column1].v);
}
//Find the shortest distance between a pixel and the neighbors from each of
//the training samples (sort of inefficient, but might do the job...sometimes)
//HSV for water sample
// learn classifier
//CvKNearest knn(trainData, trainClasses, 0, false, itemsNumber);
CvKNearest knnWaterHue(waterTrainingHue, trainClassesH, 0, false, pixelsNumber);
CvKNearest knnWaterSat(waterTrainingSat, trainClassesS, 0, false, pixelsNumber);
CvKNearest knnWaterVal(waterTrainingVal, trainClassesV, 0, false, pixelsNumber);
//HSV for ground sample
//CvKNearest knnGroundHue(groundTrainingHue, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundSat(groundTrainingSat, trainClasses2, 0, false, pixelsNumber);
//CvKNearest knnGroundVal(groundTrainingVal, trainClasses2, 0, false, pixelsNumber);
//HSV for sky sample
//if (cvmGet(skyTrainingHue,0,0)!=0.0 && cvmGet(skyTrainingSat,0,0)!=0.0 && cvmGet(skyTrainingVal,0,0)!=0.0)
//{
// CvKNearest knnSkyHue(skyTrainingHue, trainClasses, 0, false, pixelsNumber);
// CvKNearest knnSkySat(skyTrainingSat, trainClasses, 0, false, pixelsNumber);
// CvKNearest knnSkyVal(skyTrainingVal, trainClasses, 0, false, pixelsNumber);
//}
//scan nearest neighbors to each pixel
responseWaterH = knnWaterHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestWaterH,0);
responseWaterS = knnWaterSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestWaterS,0);
responseWaterV = knnWaterVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestWaterV,0);
//responseGroundH = knnGroundHue.find_nearest(sampleHue,pixelsNumber,0,0,nearestGroundH,0);
//responseGroundS = knnGroundSat.find_nearest(sampleSat,pixelsNumber,0,0,nearestGroundS,0);
//responseGroundV = knnGroundVal.find_nearest(sampleVal,pixelsNumber,0,0,nearestGroundV,0);
//for (int i=0;i<pixelsNumber;i++)
//{
for (int j=0;j<pixelsNumber;j++)
{
if ((nearestWaterH->data.fl[j] == responseWaterH) )//&& (nearestWaterH->data.fl[j] == responseWaterH + 5))
// mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if ((nearestWaterS->data.fl[j] == responseWaterS) )//&& (nearestWaterS->data.fl[j] < responseWaterS + 5))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((nearestWaterV->data.fl[j] == responseWaterV) )//&& (nearestWaterV->data.fl[j] < responseWaterV + 5))
//mark water samples as green
comparator[2] = 1;
else
comparator[2] = 0;
// similar sky pixels on the water
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
if (votesSum > 1)
{
I[x][y-6+j].h = 0;
I[x][y-6+j].s = 255;
I[x][y-6+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
//5 use to be equal to pixelsNumber-1.
y = y + 5;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
//5 use to be equal to pixelsNumber-1
x = x + 1;
y = 0;
}
// ix = 0;
}
/*********************************************************************/
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
while (x < X-1)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<6;i++)
{
column1 = y+i;
if (column1 > Y-1)
column1 = Y-1;
cvmSet(sampleHue,0,i,I[x][column1].h);
cvmSet(sampleSat,0,i,I[x][column1].s);
cvmSet(sampleVal,0,i,I[x][column1].v);
}
for (int i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
if ((minH < cvmGet(sampleHue,0,j)) && (maxH > cvmGet(sampleHue,0,j)))
//mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if ((minS < cvmGet(sampleSat,0,j)) && (maxS > cvmGet(sampleSat,0,j)))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((minV < cvmGet(sampleVal,0,j)) && (maxV > cvmGet(sampleVal,0,j)))
//mark water samples as red
comparator[2] = 1;
else
comparator[2] = 0;
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
if (votesSum > 1)
{
//use the known water samples as new training data
if((i<boatFront->height/xDivisor) && (j<boatFront->width/yDivisor))
{
cvmSet(resampleHue,i,j,cvmGet(sampleHue,0,j));
cvmSet(resampleSat,i,j,cvmGet(sampleSat,0,j));
cvmSet(resampleVal,i,j,cvmGet(sampleVal,0,j));
}
//6 use to be equal to pixelsNumber.
I[x][y-6+j].h = 0;
I[x][y-6+j].s = 255;
I[x][y-6+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
//5 use to be equal to pixelsNumber-1.
y = y + 5;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
//5 use to be equal to pixelsNumber-1
x = x + 1;
y = 0;
}
//ix = 0;
}
/***************Deal with reflection*****************/
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
votesSum = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
while (x < X-1)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i<6;i++)
{
column1 = y+i;
if (column1 > Y-1)
column1 = Y-1;
cvmSet(sampleHue,0,i,I[x][column1].h);
cvmSet(sampleSat,0,i,I[x][column1].s);
cvmSet(sampleVal,0,i,I[x][column1].v);
}
for (int i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
if ((minH < cvmGet(sampleHue,0,j)) && (maxH > cvmGet(sampleHue,0,j)))
//mark water samples as green
comparator[0] = 1;
else
comparator[0] = 0;
if ((0.8*255 > cvmGet(sampleSat,0,j)))// && (maxS < cvmGet(sampleSat,0,j)))
//mark water samples as green
comparator[1] = 1;
else
comparator[1] = 0;
if ((0.6*255 < cvmGet(sampleVal,0,j)))// || (maxV < cvmGet(sampleVal,0,j)))
//mark water samples as green
comparator[2] = 1;
else
comparator[2] = 0;
//count votes
for (int i3=0; i3 < 3; i3++)
votesSum = votesSum + comparator[i3];
if (votesSum > 1)
{
//use the known water samples as new training data
if((i<boatFront->height/xDivisor) && (j<boatFront->width/yDivisor))
{
cvmSet(resampleHue,i,j,cvmGet(sampleHue,0,j));
cvmSet(resampleSat,i,j,cvmGet(sampleSat,0,j));
cvmSet(resampleVal,i,j,cvmGet(sampleVal,0,j));
}
//6 use to be equal to pixelsNumber.
I[x][y-6+j].h = 0;
I[x][y-6+j].s = 255;
I[x][y-6+j].v = 255;
}
votesSum = 0;
}
}
if (y < Y-1)
//5 use to be equal to pixelsNumber-1.
y = y + 5;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
//5 use to be equal to pixelsNumber-1
x = x + 1;
y = 0;
}
//ix = 0;
}
/**********Resample the entire patch**********/
/*********find a new min and max for a new sample range*************/
for(int i = 0; i < 3; i++)
{
comparator[i] = 0;
}
//int counter = 0;
votesSum = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
maxH = cvmGet(resampleHue,0,0);
maxS = cvmGet(resampleSat,0,0);
maxV = cvmGet(resampleVal,0,0);
minH = cvmGet(resampleHue,0,0);
minS = cvmGet(resampleSat,0,0);
minV = cvmGet(resampleVal,0,0);
for (int i=0; i < boatFront->height/xDivisor; i++)
{
for (int j=0; j < boatFront->width/yDivisor; j++)
{
if (cvmGet(resampleHue,i,j) > maxH)
maxH = cvmGet(resampleHue,i,j);
if (cvmGet(resampleSat,i,j) > maxS)
maxS = cvmGet(resampleSat,i,j);
if (cvmGet(resampleVal,i,j) > maxV)
maxV = cvmGet(resampleVal,i,j);
if (cvmGet(resampleHue,i,j) < minH)
minH = cvmGet(resampleHue,i,j);
if (cvmGet(resampleSat,i,j) < minS)
minS = cvmGet(resampleSat,i,j);
if (cvmGet(resampleVal,i,j) < minV)
minV = cvmGet(resampleVal,i,j);
}
}
while (x < X-1)
{
for (int i=0;i<6;i++)
{
for (int j=0;j<6;j++)
{
if ((minH < I[x][y-6+j].h) && (maxH > I[x][y-6+j].h))
//mark water samples as red
I[x][y-6+j].h = 0;
else
comparator[0] = 0;
if ((minS < I[x][y-6+j].s) && (maxS > I[x][y-6+j].s))
//mark water samples as red
I[x][y-6+j].s = 255;
else
comparator[1] = 0;
if ((minV < I[x][y-6+j].v) && (maxV > I[x][y-6+j].v))
//mark water samples as red
I[x][y-6+j].v = 255;
}
}
if (y < Y-1)
//5 use to be equal to pixelsNumber-1.
y = y + 5;
if (y > Y-1)
y = Y-1;
else if (y == Y-1)
{
//5 use to be equal to pixelsNumber-1
x = x + 1;
y = 0;
}
}
//cout << "Sample data from current images" << endl;
//for (int i = 0; i<20;i++)
//{
// cout << "HUE: " << cvmGet(sampleHue,0,i) << endl;
// cout << "Saturation: " << cvmGet(sampleSat,0,i) << endl;
// cout << "Value: " << cvmGet(sampleVal,0,i) << endl;
//}
//traverse through the image one more time, divide the image in grids of
// 500x500 pixels, and see how many pixels of water are in each grid. If
// most of the pixels are labeled water, then mark all the other pixels
// as water as well
//int counter = 0;
votesSum = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
/***************Divide the picture in cells for filtering**********/
while (x < X-1)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i < boatFront->height/xDivisor; i++)
{
for(int j = 0; j < boatFront->width/yDivisor; j++)
{
cvmSet(resampleHue2,i,j,I[x+i][y+j].h);
cvmSet(resampleSat2,i,j,I[x+i][y+j].s);
cvmSet(resampleVal2,i,j,I[x+i][y+j].v);
if(cvmGet(resampleHue2,i,j)==0 && cvmGet(resampleSat2,i,j)==255 && cvmGet(resampleVal2,i,j)==255)
{
votesSum++;
}
}
}
if (votesSum > (((boatFront->height/xDivisor)*(boatFront->width/yDivisor))*(4/5)))
{
// if bigger than 4/5 the total number of pixels in a square, then consider the entire thing as water
// We might need to use other smaller quantities (like 5/6 maybe?)
for (int i = 0; i < boatFront->height/xDivisor;i++)
{
for (int j = 0; j < boatFront->width/yDivisor; j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
I[row1][column1].h = 0;
I[row1][column1].s = 255;
I[row1][column1].v = 255;
}
}
}
else
{
// If not water, eliminate all red pixels and turn those pixels
// back to the original color
for (int i = 0; i < boatFront->height/xDivisor;i++)
{
for (int j = 0; j < boatFront->width/yDivisor; j++)
{
row1 = x + i;
if (row1 > X-1)
row1 = X-1;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
I[row1][column1].h = IBackUp[row1][column1].h;//255;//IBackUp[row1][column1].h;
I[row1][column1].s = IBackUp[row1][column1].s;//255;//IBackUp[row1][column1].s;
I[row1][column1].v = IBackUp[row1][column1].v;//255;//IBackUp[row1][column1].v;
}
}
}
y = y + boatFront->width/xDivisor;
if (y > Y-1)
{
x = x + boatFront->height/yDivisor;
y = 0;
}
votesSum = 0;
}
/********************Isolate obstacles************************/
votesSum = 0;
int paint = 0;
column1 = 0;
row1 = 0;
x = boatFront->height/1.45;
y = 0;
xDiv = 20;
yDiv = 20;
/***************Divide the picture in cells for filtering**********/
// Small pixel areas (noise) are going to be eliminated from the picture
// living only the big obstacles
while (x < X-2)
{
//get a random sample taken from the picture. Must be determined whether
//it is water or ground
for (int i = 0; i < boatFront->height/xDiv; i++)
{
for(int j = 0; j < boatFront->width/yDiv; j++)
{
row1 = x + i;
if (row1 > X-2)
row1 = X-2;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
cvmSet(resampleHue2,i,j,I[row1][column1].h);
cvmSet(resampleSat2,i,j,I[row1][column1].s);
cvmSet(resampleVal2,i,j,I[row1][column1].v);
if(cvmGet(resampleHue2,i,j)==0 && cvmGet(resampleSat2,i,j)==255 && cvmGet(resampleVal2,i,j)==255)
{
votesSum++;
}
}
}
if (votesSum > (((boatFront->height/xDiv)*(boatFront->width/yDiv))*(4.5/5)))
{
// if bigger than 4/5 the total number of pixels in a square, then consider the entire thing as water
// We might need to use other smaller quantities (like 5/6 maybe?)
for (int i = 0; i < boatFront->height/xDiv;i++)
{
for (int j = 0; j < boatFront->width/yDiv; j++)
{
row1 = x + i;
if (row1 > X-2)
row1 = X-2;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
I[row1][column1].h = 0;
I[row1][column1].s = 255;
I[row1][column1].v = 255;
}
}
}
else
{
int count = 0;
// If not water, eliminate all red pixels and turn those pixels
// back to the original color
for (int i = 0; i < boatFront->height/xDiv;i++)
{
for (int j = 0; j < boatFront->width/yDiv; j++)
{
row1 = x + i;
if (row1 > X-2)
row1 = X-2;
column1 = y+j;
if (column1 > Y-1)
column1 = Y-1;
I[row1][column1].h = IBackUp[row1][column1].h;//255;
I[row1][column1].s = IBackUp[row1][column1].s;//255;
I[row1][column1].v = IBackUp[row1][column1].v;//255;
// count++;
}
}
}
y = y + boatFront->width/yDiv;
if (y > Y-1)
{
x = x + boatFront->height/xDiv;
if (x > X-2)
x = X-2;
y = 0;
}
votesSum = 0;
}
/****************Find Obstacles boundaries*********************************/
if( grayStorage == NULL )
{
grayStorage = cvCreateMemStorage(0);
}
else
{
cvClearMemStorage(grayStorage);
}
backUpImage = cvCloneImage(boatFront);
//convert from HSV to RGB
cvCvtColor(boatFront, boatFront, CV_HSV2BGR);
cvCvtColor(backUpImage, backUpImage, CV_HSV2BGR);
//do flood fill for obstacles
cvFloodFill( backUpImage, seed_point, color, cvScalarAll(255), cvScalarAll(2), NULL, 8, NULL);
//convert to to gray to do more obstacle segmentation
cvCvtColor(backUpImage, grayImage, CV_BGR2GRAY);
//convert to binary
cvThreshold(grayImage, bwImage, 100, 255, CV_THRESH_BINARY | CV_THRESH_OTSU);
//eliminate small unnecessary pixel areas
//bwImage is a pointer, so no need to reuse findCountours
int findCountours = bwareaopen_(bwImage, 100);
//find contours of obstacles in image
cvFindContours(bwImage, grayStorage, &contours);
cvZero( bwImage ); //redraw clean contours
for( CvSeq* c=contours; c!=NULL; c=c->h_next)
{
cvDrawContours(bwImage, c, cvScalarAll(255), cvScalarAll(255), 8);
cout << "Contour area: " << cvContourArea(c, CV_WHOLE_SEQ) << endl; //area in pixels
//find the x,y coordinate of the center of a contour
cvMoments(c, &moment, 0);
//centroid/moment of the contour/obstacle
//cout << "Contour center: " << moment.m10/moment.m00 << ", " << moment.m01/moment.m00 << endl;
//The distance formula calculated by plotting points is given by:
/*********** distance = 0.1622208546*pow(1.0186851612,pixels) *****************/
/*********** pixel = 87.0413255*pow(distance,0.4062956891) *****************/
//These formulas only work for 640X480 images
// x,y coordinates of the obstacle from the bottom center of the image
//Ignore everything less than 0.3 meters apart (anything too close to the boat)
//if ((X - (row1 -(boatFront->height/xDiv)/2)) > (87.0413255*pow(0.3,0.4062956891)))
//{
xObstacleDistance = 0.1622208546*pow(1.0186851612,X - (moment.m10/moment.m00));
if (xObstacleDistance == 0.0) //try to ignore obstacle that are too close
xObstacleDistance = 0.01; //robot shall tell operator if there is
//a problem with a close by obstacle
yObstacleDistance = 0.1622208546*pow(1.0186851612,Y/2 - (moment.m01/moment.m00));
//obstacle distance
obstacleDistance = sqrt(pow(xObstacleDistance,2) + pow(yObstacleDistance,2));
//obstacle heading
obstacleHeading = tan((yObstacleDistance/xObstacleDistance)*PI/180);
cout << "Obstacle polar coordinates: " << endl;
cout << "x: " << xObstacleDistance << " Y: " << yObstacleDistance << endl;
cout << "Distance (meters) " << obstacleDistance << endl;
cout << "Direction (degrees): " << obstacleHeading << endl << endl;
//}
}
/**************************************************************************/
try
{
//fprintf(stderr,"\n boatFront\n");
cvShowImage("Boat Front", boatFront);
//cvShowImage("Color Segment", backUpImage);
//cvShowImage("Obstacles", bwImage);
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
