//Add a wall plane facing in a direction
void AddWall(scene& scene1, vector3 pos, vector3 dir, rgbf col =rgbf(0, 0.5, 0.5))
{
plane_object* plane1 = new plane_object(pos, vector3(0, 0, 1)^dir);
plane1->natrual_colour = col;
plane1->k_spec = 0;
scene1.add_object(plane1);
}
void AddBox(scene& scene1, vector3 pos, float r =0.5f, rgbf col =rgbf(0, 0, 1), bool ref =false)
{
box* box1 = new box(pos, vector3(-r, -r, -r), vector3(r, r, r));
box1->natrual_colour = col;
box1->reflective = ref;
scene1.add_object(box1);
}
//Add a sphere
void AddSphere(scene& scene1, vector3 pos, float r =0.5f, rgbf col =rgbf(0, 0, 1), bool ref =false)
{
sphere_object* sphere = new sphere_object(r, pos);
sphere->natrual_colour = col;
sphere->reflective = ref;
scene1.add_object(sphere);
}
//Add a ceiling
void AddCeiling(scene& scene1, vector3 pos, rgbf col =rgbf(0, 1, 0))
{
plane_object* plane1 = new plane_object(pos, vector3(0, 0, -1));
plane1->natrual_colour = col;
plane1->k_spec = 0; //No specular on floors
scene1.add_object(plane1);
}
void AddLightBall(scene& scene1, vector3 pos, int number, float r)
{
for (int i = 0; i < number; i++)
{
float t = 2 * M_PI / number * i;
light* lig = new light(pos + vector3(r * cos(t), r * sin(t), 0), rgbf(1, 1, 1) , 1.0 / number);
scene1.add_light(lig);
}
}
//Add triangle to mesh with normal
void ATTMWN(mesh* m, vector3 v1, vector3 v2, vector3 v3, vector3 n)
{
triangle_object* tri = new triangle_object(v1, v1 - v1, v2 - v1, v3 - v1, n);
tri->natrual_colour = rgbf(1, 1, 1);
m->add_triangle(tri);
}
void AddTriangleToMesh(mesh* m, vector3 v1, vector3 v2, vector3 v3)
{
triangle_object* tri = new triangle_object(v1, v1 - v1, v2 - v1, v3 - v1);
tri->natrual_colour = rgbf(1, 1, 1);
m->add_triangle(tri);
}
void AddTriangle(scene& scene1, vector3 v1, vector3 v2, vector3 v3)
{
triangle_object* tri = new triangle_object(v1, v1 - v1, v2 - v1, v3 - v1);
tri->natrual_colour = rgbf(1, 1, 1);
scene1.add_object(tri);
}
void TestFunction1(scene& scene1)
{
AddSphere(scene1, vector3(1.5, -1.5, 0), 0.5, rgbf(0, 0, 1), false);
//AddSphere(scene1, vector3(1, 1, 0), 0.5, rgbf(0, 1, 1), true);
sphere_object* s1 = new sphere_object(0.5, vector3(1.5, -1, 0));
s1->natrual_colour = rgbf(0, 0, 0);
s1->reflective = true;
s1->I_refl = 1;
s1->k_spec = 1;
s1->ambient_colour = rgbf(0, 0, 0);
s1->shininess = 100;
//scene1.add_object(s1);
sphere_object* s2 = new sphere_object(0.5, vector3(1.5, 1, 0));
s2->transparent = true;
s2->transparency = 0.9;
s2->refindex = 0.9;
scene1.add_object(s2);
rgbf red(1, 0, 0);
rgbf green(0, 1, 0);
rgbf blue(0, 0, 1);
rgbf yellow = red + green;
rgbf magenta = red + blue;
rgbf cyan = green + blue;
AddWall(scene1, vector3(3, 0, -0.5), vector3(0, 1, 0), green * 0.7);
AddWall(scene1, vector3(0, 3, -0.5), vector3(-1, 0, 0), red* 0.7);
AddWall(scene1, vector3(-3, 0, -0.5), vector3(0, -1, 0), blue* 0.7);
AddWall(scene1, vector3(0, -3, -0.5), vector3(1, 0, 0), yellow* 0.7);
AddCeiling(scene1, vector3(0, 0, +3.5), magenta * 0.7);
AddFloor(scene1, vector3(0, 0, -0.5), cyan * 0.7);
//AddBox(scene1, vector3(1, -0.5, -0.3), 0.2, rgbf(1, 0, 1), true);
box* box1 = new box(vector3(1, -0.5, 0), vector3(-0.2, -0.2, -0.2), vector3(0.2, 0.2, 0.2));
box1->natrual_colour = rgbf(0, 0, 0);
box1->reflective = false;
box1->transparent = true;
box1->transparency = 1;
box1->I_refr = 1;
box1->refindex = 2;
scene1.add_object(box1);
//AddLightBall(scene1, vector3(-0.8, 0, 3), 3, 0.1);
AddLight(scene1, vector3(-1.5, 1.5, 3));
//AddLight(scene1, vector3(2, 0, 4));
//AddLight(scene1, vector3(-5, 3, 2));
//IcoSphere(scene1, vector3(4, 0, 2));
torus_object* tor1 = new torus_object(0.1,0.5,vector3(1,0,-0.5));
tor1->natrual_colour = rgbf(1, 0, 0);
//tor1->ambient_colour = rgbf(0, 0, 0.5);
//scene1.add_object(tor1);
mesh* m1 = ReadMesh("teapot.obj", vector3(1, -0.25, -0.5));
m1->natrual_colour = rgbf(0, 0, 0);
m1->reflective = false;
m1->transparent = true;
m1->transparency = 1;
m1->I_refr = 1;
//scene1.add_object(m1);
}
//Add a point light source
void AddLight(scene& scene1, vector3 pos)
{
scene1.add_light(new light(pos, rgbf(1, 1, 1), 1.0f));
}
// ***************************************************************************
void CMeshMorpher::update (std::vector<CAnimatedMorph> *pBSFactor)
{
uint32 i, j;
if (_VBOri == NULL)
return;
if (BlendShapes.size() == 0)
return;
if (_VBOri->getNumVertices() != _VBDst->getNumVertices())
{ // Because the original vertex buffer is not initialized by default
// we must init it here (if there are some blendshapes)
*_VBOri = *_VBDst;
}
// Does the flags are reserved ?
if (_Flags.size() != _VBOri->getNumVertices())
{
_Flags.resize (_VBOri->getNumVertices());
for (i = 0; i < _Flags.size(); ++i)
_Flags[i] = Modified; // Modified to update all
}
nlassert(_VBOri->getVertexFormat() == _VBDst->getVertexFormat());
// Cleaning with original vertex buffer
uint32 VBVertexSize = _VBOri->getVertexSize();
CVertexBufferRead srcvba;
_VBOri->lock (srcvba);
CVertexBufferReadWrite dstvba;
_VBDst->lock (dstvba);
const uint8 *pOri = (const uint8*)srcvba.getVertexCoordPointer ();
uint8 *pDst = (uint8*)dstvba.getVertexCoordPointer ();
for (i= 0; i < _Flags.size(); ++i)
if (_Flags[i] >= Modified)
{
_Flags[i] = OriginalVBDst;
for(j = 0; j < VBVertexSize; ++j)
pDst[j+i*VBVertexSize] = pOri[j+i*VBVertexSize];
}
uint tgSpaceStage = 0;
if (_UseTgSpace)
{
tgSpaceStage = _VBDst->getNumTexCoordUsed() - 1;
}
// Blending with blendshape
for (i = 0; i < BlendShapes.size(); ++i)
{
CBlendShape &rBS = BlendShapes[i];
float rFactor = pBSFactor->operator[](i).getFactor()/100.0f;
// todo hulud check it works
// if (rFactor > 0.0f)
if (rFactor != 0.0f)
for (j = 0; j < rBS.VertRefs.size(); ++j)
{
uint32 vp = rBS.VertRefs[j];
// Modify Pos/Norm/TgSpace.
//------------
if (_VBDst->getVertexFormat() & CVertexBuffer::PositionFlag)
if (rBS.deltaPos.size() > 0)
{
CVector *pV = dstvba.getVertexCoordPointer (vp);
*pV += rBS.deltaPos[j] * rFactor;
}
if (_VBDst->getVertexFormat() & CVertexBuffer::NormalFlag)
if (rBS.deltaNorm.size() > 0)
{
CVector *pV = dstvba.getNormalCoordPointer (vp);
*pV += rBS.deltaNorm[j] * rFactor;
}
if (_UseTgSpace)
if (rBS.deltaTgSpace.size() > 0)
{
CVector *pV = (CVector*)dstvba.getTexCoordPointer (vp, tgSpaceStage);
*pV += rBS.deltaTgSpace[j] * rFactor;
}
// Modify UV0 / Color
//------------
if (_VBDst->getVertexFormat() & CVertexBuffer::TexCoord0Flag)
if (rBS.deltaUV.size() > 0)
{
CUV *pUV = dstvba.getTexCoordPointer (vp);
*pUV += rBS.deltaUV[j] * rFactor;
}
if (_VBDst->getVertexFormat() & CVertexBuffer::PrimaryColorFlag)
if (rBS.deltaCol.size() > 0)
{
// todo hulud d3d vertex color RGBA / BGRA
CRGBA *pRGBA = (CRGBA*)dstvba.getColorPointer (vp);
CRGBAF rgbf(*pRGBA);
rgbf.R += rBS.deltaCol[j].R * rFactor;
rgbf.G += rBS.deltaCol[j].G * rFactor;
rgbf.B += rBS.deltaCol[j].B * rFactor;
rgbf.A += rBS.deltaCol[j].A * rFactor;
clamp(rgbf.R, 0.0f, 1.0f);
clamp(rgbf.G, 0.0f, 1.0f);
clamp(rgbf.B, 0.0f, 1.0f);
clamp(rgbf.A, 0.0f, 1.0f);
*pRGBA = rgbf;
}
// Modified
_Flags[vp] = Modified;
}
}
}
void demo::makemap_ICP(){
ros::Time start, end;
Eigen::Matrix4d T_backup;
T_backup.setIdentity();
PCloud cloud_s;
PCloud cloud_d;
//take 1 frame
cloud_s = getframe();
pub_pointcloud.publish(cloud_s);
std::cout<< 0 << "\n";
*cloud_key = *cloud_s;
cloud_d = getframe();
ICP icp(cloud_s,cloud_d);
RGBFeaturesInit rgbf(cloud_s,cloud_d);
rgbf_key.setRansacThreshold(th);
rgbf_key.setDesiredP(prob);
rgbf.setRansacThreshold(th);
rgbf.setDesiredP(prob);
icp.setRThreshold (THRESH_dr);
icp.setTThreshold (THRESH_dt);
icp.setDisThreshold(D);
icp.setNiterations (max_iterations);
icp.setMaxPairs (MAXPAIRS);
icp.setMinPairs (MINPAIRS);
Eigen::Vector3d p1(0.0,0.0,0.0);
Eigen::Vector3d p2(0.0,0.0,0.1);
camera_positions.push_back(std::make_pair(p1, p2));
int i=1;
float a1,a2;
int ft;
while(ros::ok()) {
ROS_INFO("%d ", i);
start = ros::Time::now();
ft = rgbf.compute();
end = ros::Time::now();
a1 = (end-start).toSec();
start = ros::Time::now();
if ( ft > f_th ){
icp.align(rgbf.T,rgbf.getInliers());
}else{
icp.align(T_backup);
}
end = ros::Time::now();
a2 = (end-start).toSec();
//update transformation
T_backup=icp.returnTr();
T = T*icp.returnTr();
ROS_INFO("RGB: %.6f seconds | ICP: %.6f seconds | inliears= %d", a1, a2,ft);
//update camera position
p1 = T.block<3,3>(0,0)*camera_positions[0].first + T.block<3,1>(0,3);
p2 = T.block<3,3>(0,0)*camera_positions[0].second + T.block<3,1>(0,3);
camera_positions.push_back(std::make_pair(p1, p2));
if(checkIfIsNewKeyframe(cloud_d)){
ApplyRTto(cloud_d);
pub_pointcloud.publish(cloud_d);
}
cloud_d = getframe();
//update icp clouds
rgbf.setNewinputCloud(cloud_d);
icp.setNewICPinputCloud(cloud_d);
i++;
}
WriteCameraPLY(cameras_filename.data(), camera_positions);
}
