Mat LRTV::compute()
{
int max_iter = 30;
for(int iter = 0; iter < max_iter; ++iter)
{
cout << "=====================" << endl;
cout << "Iter = " << (iter + 1) << endl;
sub_1();
sub_2();
sub_3();
// when to stop needs further consideration
if( iter >= 5 && norm(U_, U_last_) / norm(U_last_) < 1.5e-3 )
{
break;
}
cout << "relative error = " << 1.0* norm(U_, U_last_) / norm(U_last_) << endl;
U_.copyTo(U_last_);
cout << "PSNR = " << PSNR(I_, U_, mask_) << endl;
cout << endl;
}
return U_;
}
TEST(CanAcceptanceFilter, Basic_test)
{
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::camera_gimbal::AngularCommand> _reg1;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::Sideslip> _reg2;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::TrueAirspeed> _reg3;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::AngleOfAttack> _reg4;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::ahrs::Solution> _reg5;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::StaticPressure> _reg6;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::file::BeginFirmwareUpdate> _reg7;
SystemClockDriver clock_driver;
CanDriverMock can_driver(1, clock_driver);
TestNode node(can_driver, clock_driver, 24);
uavcan::Subscriber<uavcan::equipment::camera_gimbal::AngularCommand,
SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand>::ExtendedBinder> sub_1(node);
uavcan::Subscriber<uavcan::equipment::air_data::Sideslip,
SubscriptionListener<uavcan::equipment::air_data::Sideslip>::ExtendedBinder> sub_2(node);
uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed,
SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed>::ExtendedBinder> sub_3(node);
uavcan::Subscriber<uavcan::equipment::air_data::AngleOfAttack,
SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack>::ExtendedBinder> sub_4(node);
uavcan::Subscriber<uavcan::equipment::ahrs::Solution,
SubscriptionListener<uavcan::equipment::ahrs::Solution>::ExtendedBinder> sub_5(node);
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6(node);
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6_1(node);
uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> server(node);
SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand> listener_1;
SubscriptionListener<uavcan::equipment::air_data::Sideslip> listener_2;
SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed> listener_3;
SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack> listener_4;
SubscriptionListener<uavcan::equipment::ahrs::Solution> listener_5;
SubscriptionListener<uavcan::equipment::air_data::StaticPressure> listener_6;
sub_1.start(listener_1.bindExtended());
sub_2.start(listener_2.bindExtended());
sub_3.start(listener_3.bindExtended());
sub_4.start(listener_4.bindExtended());
sub_5.start(listener_5.bindExtended());
sub_6.start(listener_6.bindExtended());
sub_6_1.start(listener_6.bindExtended());
server.start(writeServiceServerCallback);
std::cout << "Subscribers are initialized ..." << std::endl;
uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node);
int configure_filters_assert = anon_test_configuration.configureFilters();
if (configure_filters_assert == 0)
{
std::cout << "Filters are configured with anonymous configuration..." << std::endl;
}
const auto& configure_array = anon_test_configuration.getConfiguration();
uint32_t configure_array_size = configure_array.getSize();
ASSERT_EQ(configure_filters_assert, 0);
ASSERT_EQ(configure_array_size, 4);
for (uint16_t i = 0; i<configure_array_size; i++)
{
std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl;
std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
}
ASSERT_EQ(configure_array.getByIndex(0)->id, 0);
ASSERT_EQ(configure_array.getByIndex(0)->mask, 255);
ASSERT_EQ(configure_array.getByIndex(1)->id, 256000);
ASSERT_EQ(configure_array.getByIndex(1)->mask, 16771968);
ASSERT_EQ(configure_array.getByIndex(2)->id, 6272);
ASSERT_EQ(configure_array.getByIndex(2)->mask, 32640);
ASSERT_EQ(configure_array.getByIndex(3)->id, 262144);
ASSERT_EQ(configure_array.getByIndex(3)->mask, 16771200);
uavcan::CanAcceptanceFilterConfigurator no_anon_test_confiruration(node);
configure_filters_assert = no_anon_test_confiruration.configureFilters
(uavcan::CanAcceptanceFilterConfigurator::IgnoreAnonymousMessages);
if (configure_filters_assert == 0)
{
std::cout << "Filters are configured without anonymous configuration..." << std::endl;
}
const auto& configure_array_2 = no_anon_test_confiruration.getConfiguration();
configure_array_size = configure_array_2.getSize();
ASSERT_EQ(configure_filters_assert, 0);
ASSERT_EQ(configure_array_size, 4);
for (uint16_t i = 0; i<configure_array_size; i++)
{
std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl;
std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
}
ASSERT_EQ(configure_array_2.getByIndex(0)->id, 6272);
ASSERT_EQ(configure_array_2.getByIndex(0)->mask, 32640);
ASSERT_EQ(configure_array_2.getByIndex(1)->id, 262144);
ASSERT_EQ(configure_array_2.getByIndex(1)->mask, 16776320);
ASSERT_EQ(configure_array_2.getByIndex(2)->id, 256000);
ASSERT_EQ(configure_array_2.getByIndex(2)->mask, 16771968);
ASSERT_EQ(configure_array_2.getByIndex(3)->id, 262144);
ASSERT_EQ(configure_array_2.getByIndex(3)->mask, 16771968);
}
TEST(CanAcceptanceFilter, Basic_test)
{
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::camera_gimbal::AngularCommand> _reg1;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::Sideslip> _reg2;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::TrueAirspeed> _reg3;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::AngleOfAttack> _reg4;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::ahrs::AHRS> _reg5;
uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::StaticPressure> _reg6;
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::file::BeginFirmwareUpdate> _reg7;
SystemClockDriver clock_driver;
CanDriverMock can_driver(1, clock_driver);
TestNode node(can_driver, clock_driver, 24);
uavcan::Subscriber<uavcan::equipment::camera_gimbal::AngularCommand,
SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand>::ExtendedBinder> sub_1(node);
uavcan::Subscriber<uavcan::equipment::air_data::Sideslip,
SubscriptionListener<uavcan::equipment::air_data::Sideslip>::ExtendedBinder> sub_2(node);
uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed,
SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed>::ExtendedBinder> sub_3(node);
uavcan::Subscriber<uavcan::equipment::air_data::AngleOfAttack,
SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack>::ExtendedBinder> sub_4(node);
uavcan::Subscriber<uavcan::equipment::ahrs::AHRS,
SubscriptionListener<uavcan::equipment::ahrs::AHRS>::ExtendedBinder> sub_5(node);
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6(node);
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure,
SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6_1(node);
uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> server(node);
SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand> listener_1;
SubscriptionListener<uavcan::equipment::air_data::Sideslip> listener_2;
SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed> listener_3;
SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack> listener_4;
SubscriptionListener<uavcan::equipment::ahrs::AHRS> listener_5;
SubscriptionListener<uavcan::equipment::air_data::StaticPressure> listener_6;
sub_1.start(listener_1.bindExtended());
sub_2.start(listener_2.bindExtended());
sub_3.start(listener_3.bindExtended());
sub_4.start(listener_4.bindExtended());
sub_5.start(listener_5.bindExtended());
sub_6.start(listener_6.bindExtended());
sub_6_1.start(listener_6.bindExtended());
server.start(writeServiceServerCallback);
std::cout << "Subscribers are initialized ..." << std::endl;
uavcan::CanAcceptanceFilterConfigurator test_configurator(node);
int configure_filters_assert = test_configurator.configureFilters();
if (configure_filters_assert == 0)
{
std::cout << "Filters are configured ..." << std::endl;
}
const auto& configure_array = test_configurator.getConfiguration();
uint32_t configure_array_size = configure_array.getSize();
ASSERT_EQ(configure_filters_assert, 0);
ASSERT_EQ(configure_array_size, 4);
for (uint16_t i = 0; i<configure_array_size; i++)
{
std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl;
std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
}
ASSERT_EQ(configure_array.getByIndex(0)->id, 268435456);
ASSERT_EQ(configure_array.getByIndex(0)->mask, 469762048);
ASSERT_EQ(configure_array.getByIndex(1)->id, 363069440);
ASSERT_EQ(configure_array.getByIndex(1)->mask, 536739840);
ASSERT_EQ(configure_array.getByIndex(2)->id, 16777216);
ASSERT_EQ(configure_array.getByIndex(2)->mask, 124452864);
ASSERT_EQ(configure_array.getByIndex(3)->id, 18874368);
ASSERT_EQ(configure_array.getByIndex(3)->mask, 133169152);
}
int main() {
return sub_3(10, 4, 2);
}
static int _ggi_clip2d_3(ggi_visual *vis,
int *_x0, int *_y0,
int *_x1, int *_y1,
int *clip_first, int *clip_last)
{
int first,last, code;
int x0,y0,x1,y1;
int x,y;
unsigned dx[3], dy[3], tmp[3];
unsigned int absdx, absdy;
int xmajor;
int slope;
int i;
*clip_first = first = 0;
*clip_last = last = 0;
outcode(first,*_x0,*_y0);
outcode(last,*_x1,*_y1);
if ((first | last) == 0) {
return 1; /* Trivially accepted! */
}
if ((first & last) != 0) {
return 0; /* Trivially rejected! */
}
x0=*_x0; y0=*_y0;
x1=*_x1; y1=*_y1;
assign_int_3(dx, x1);
assign_int_3(tmp, x0);
sub_3(dx, tmp);
assign_int_3(dy, y1);
assign_int_3(tmp, y0);
sub_3(dy, tmp);
absdx = x0 < x1 ? x1 - x0 : x0 - x1;
absdy = y0 < y1 ? y1 - y0 : y0 - y1;
xmajor = absdx > absdy;
slope = ((x1>=x0) && (y1>=y0)) || ((x1<x0) && (y1<y0));
for (i = 0; i < 4; i++) {
code = first;
if (first==0)
code = last;
if (code&OC_LEFT) {
x = LIBGGI_GC(vis)->cliptl.x;
if (xmajor) {
/* y = *_y0 + FloorDiv(dy*(x - *_x0)*2 + dx,
2*dx); */
unsigned _x[3], res[3];
assign_int_3(_x, x);
assign_int_3(tmp, *_x0);
sub_3(_x, tmp);
lshift_3(_x, 1);
mul_3(_x, dy);
add_3(_x, dx);
assign_3(tmp, dx);
lshift_3(tmp, 1);
FloorDiv_3(res, _x, tmp);
assign_int_3(tmp, *_y0);
add_3(res, tmp);
y = res[0];
} else if (slope) {
/* y = *_y0 + CeilDiv(dy*((x - *_x0)*2 - 1),
2*dx); */
unsigned _x[3], res[3];
assign_int_3(_x, x);
assign_int_3(tmp, *_x0);
sub_3(_x, tmp);
lshift_3(_x, 1);
dec_3(_x);
mul_3(_x, dy);
assign_3(tmp, dx);
lshift_3(tmp, 1);
CeilDiv_3(res, _x, tmp);
assign_int_3(tmp, *_y0);
add_3(res, tmp);
y = res[0];
} else {
/* y = *_y0 + FloorDiv(dy*((x - *_x0)*2 - 1),
2*dx); */
unsigned _x[3], res[3];
assign_int_3(_x, x);
assign_int_3(tmp, *_x0);
sub_3(_x, tmp);
lshift_3(_x, 1);
dec_3(_x);
mul_3(_x, dy);
assign_3(tmp, dx);
lshift_3(tmp, 1);
FloorDiv_3(res, _x, tmp);
assign_int_3(tmp, *_y0);
add_3(res, tmp);
y = res[0];
}
} else if (code&OC_RIGHT) {
x = LIBGGI_GC(vis)->clipbr.x - 1;
if (xmajor) {
/* y = *_y0 + FloorDiv(dy*(x - *_x0)*2 + dx,
2*dx); */
unsigned _x[3], res[3];
assign_int_3(_x, x);
assign_int_3(tmp, *_x0);
sub_3(_x, tmp);
lshift_3(_x, 1);
mul_3(_x, dy);
add_3(_x, dx);
assign_3(tmp, dx);
lshift_3(tmp, 1);
FloorDiv_3(res, _x, tmp);
assign_int_3(tmp, *_y0);
add_3(res, tmp);
y = res[0];
} else if (slope) {
/* y = *_y0 + CeilDiv(dy*((x - *_x0)*2 + 1),
2*dx)-1; */
unsigned _x[3], res[3];
assign_int_3(_x, x);
assign_int_3(tmp, *_x0);
sub_3(_x, tmp);
lshift_3(_x, 1);
inc_3(_x);
mul_3(_x, dy);
assign_3(tmp, dx);
lshift_3(tmp, 1);
CeilDiv_3(res, _x, tmp);
dec_3(res);
assign_int_3(tmp, *_y0);
add_3(res, tmp);
y = res[0];
} else {
/* y = *_y0 + FloorDiv(dy*((x - *_x0)*2 + 1),
2*dx)+1; */
unsigned _x[3], res[3];
assign_int_3(_x, x);
assign_int_3(tmp, *_x0);
sub_3(_x, tmp);
lshift_3(_x, 1);
inc_3(_x);
mul_3(_x, dy);
assign_3(tmp, dx);
lshift_3(tmp, 1);
FloorDiv_3(res, _x, tmp);
inc_3(res);
assign_int_3(tmp, *_y0);
add_3(res, tmp);
y = res[0];
}
} else if (code&OC_TOP) {
y = LIBGGI_GC(vis)->cliptl.y;
if (!xmajor) {
/* x = *_x0 + FloorDiv(dx*(y - *_y0)*2 + dy,
2*dy); */
unsigned _y[3], res[3];
assign_int_3(_y, y);
assign_int_3(tmp, *_y0);
sub_3(_y, tmp);
lshift_3(_y, 1);
mul_3(_y, dx);
add_3(_y, dy);
assign_3(tmp, dy);
lshift_3(tmp, 1);
FloorDiv_3(res, _y, tmp);
assign_int_3(tmp, *_x0);
add_3(res, tmp);
x = res[0];
} else if (slope) {
/* x = *_x0 + CeilDiv(dx*((y - *_y0)*2 - 1),
2*dy); */
unsigned _y[3], res[3];
assign_int_3(_y, y);
assign_int_3(tmp, *_y0);
sub_3(_y, tmp);
lshift_3(_y, 1);
dec_3(_y);
mul_3(_y, dx);
assign_3(tmp, dy);
lshift_3(tmp, 1);
CeilDiv_3(res, _y, tmp);
assign_int_3(tmp, *_x0);
add_3(res, tmp);
x = res[0];
} else {
/* x = *_x0 + FloorDiv(dx*((y - *_y0)*2 - 1),
2*dy); */
unsigned _y[3], res[3];
assign_int_3(_y, y);
assign_int_3(tmp, *_y0);
sub_3(_y, tmp);
lshift_3(_y, 1);
dec_3(_y);
mul_3(_y, dx);
assign_3(tmp, dy);
lshift_3(tmp, 1);
FloorDiv_3(res, _y, tmp);
assign_int_3(tmp, *_x0);
add_3(res, tmp);
x = res[0];
}
} else { /* OC_BOTTOM */
LIB_ASSERT((code & OC_BOTTOM), "unknown outcode\n");
y = LIBGGI_GC(vis)->clipbr.y - 1;
if (!xmajor) {
/* x = *_x0 + FloorDiv(dx*(y - *_y0)*2 + dy,
2*dy); */
unsigned _y[3], res[3];
assign_int_3(_y, y);
assign_int_3(tmp, *_y0);
sub_3(_y, tmp);
lshift_3(_y, 1);
mul_3(_y, dx);
add_3(_y, dy);
assign_3(tmp, dy);
lshift_3(tmp, 1);
FloorDiv_3(res, _y, tmp);
assign_int_3(tmp, *_x0);
add_3(res, tmp);
x = res[0];
} else if (slope) {
/* x = *_x0 + CeilDiv(dx*((y - *_y0)*2 + 1),
2*dy)-1; */
unsigned _y[3], res[3];
assign_int_3(_y, y);
assign_int_3(tmp, *_y0);
sub_3(_y, tmp);
lshift_3(_y, 1);
inc_3(_y);
mul_3(_y, dx);
assign_3(tmp, dy);
lshift_3(tmp, 1);
CeilDiv_3(res, _y, tmp);
dec_3(res);
assign_int_3(tmp, *_x0);
add_3(res, tmp);
x = res[0];
} else {
/* x = *_x0 + FloorDiv(dx*((y - *_y0)*2 + 1),
2*dy)+1; */
unsigned _y[3], res[3];
assign_int_3(_y, y);
assign_int_3(tmp, *_y0);
sub_3(_y, tmp);
lshift_3(_y, 1);
inc_3(_y);
mul_3(_y, dx);
assign_3(tmp, dy);
lshift_3(tmp, 1);
FloorDiv_3(res, _y, tmp);
inc_3(res);
assign_int_3(tmp, *_x0);
add_3(res, tmp);
x = res[0];
}
}
if (first!=0) {
x0 = x;
y0 = y;
outcode(first,x0,y0);
*clip_first = 1;
} else {
x1 = x;
y1 = y;
last = code;
outcode(last,x1,y1);
*clip_last = 1;
}
if ((first & last) != 0) {
return 0; /* Trivially rejected! */
}
if ((first | last) == 0) {
*_x0=x0; *_y0=y0;
*_x1=x1; *_y1=y1;
return 1; /* Trivially accepted! */
}
}
return 0; /* Aieee! Failed to clip, clip whole line... */
}
